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1  Foreword 


The  ultimate  objective  of  this  work  is  to  design  planning  and  control  archi¬ 
tectures  that  enable  autonomous  vehicles  to  operate  in  open  terrain  with¬ 
out  sacrificing  speed  and  maneuverability.  To  this  end,  we  develop  mobility 
metrics  for  UGVs  operating  off-road  at  high-speed  regimes,  explore  optimal 
algorithms  to  derive  optimal  paths  for  UGVs,  and  propose  a  framework  for 
motion  planning.  The  major  tasks  to  accomplish  this  goal  are  the  following: 

•  Develop  an  efficient  stochastic  terrain  traversal  prediction  tool  that 
allows  systematic  assessment  of  the  ability  of  a  vehicle  to  negotiate 
challenging  terrain.  This  tool  will  improve  the  computational  efficiency 
of  current  Monte  Carlo-based  methods  and  allow  for  the  construction 
of  a  large  library  of  obstacle  traversal  maneuvers. 


*  Design  vehicle  mobility  metrics  that  quantify  the  ability  of  a  vehicle 
to  traverse  challenging  terrain  and  to  track  trajectories  with  varying 
curvature  and  speed.  These  metrics  will  be  unique  since  they  will:  a) 
represent  a  UGVs  obstacle  traversal  capability  as  a  function  of  its  tra¬ 
jectory  (i.e.,  path  and  velocity),  and  b)  consider  UGV  agility  (i.e.,  the 
ability  to  track  high-curvature  paths,  and  execute  challenging  maneu¬ 
vers)  . 

•  Develop  methods  for  evaluation  of  terrain/control  strategy  pairs  that 
provide  a  compact  yet  accurate  way  to  identify,  classify,  and  assess 
terrain-specific  maneuvers.  This  task  will  bridge  the  gap  between  ac¬ 
curate  but  complex  metrics,  and  tractable  metrics  that  can  be  used  in 
real-time  path  planning. 


•  Develop  a  symbolic  control  framework  that  permits  the  execution  of 
complex,  high-performance  trajectories  through  the  on-line  combina¬ 
tion  of  a  number  of  baseline  controllers,  chosen  from  a  finite  library  of 
elementary  behaviors  or  maneuvers. 
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3  Problem  Statement 


In  this  project,  we  consider  the  problem  of  autonomous  driving  of  land  vehi¬ 
cles  in  open  terrain  at  high  speeds.  Achieving  autonomous  operation  of  these 
vehicles  in  open  terrain  remains  a  a  challenging  problem  not  only  due  to  un¬ 
certainty  of  the  operating  environment  but  also  due  to  the  limited  knowledge 
of  the  vehicle’s  mobility  in  such  type  of  terrain.  Moreover,  autonomous  driv¬ 
ing  of  wheeled  vehicles  at  high  speeds  adds  a  new  level  of  complexity  due  to 
the  time  constraints  imposed  by  the  small  reaction  times  and  the  nonlinear 
characteristics  of  the  vehicle’s  operation.  A  typical  scenario  for  autonomous 
driving  in  open  terrain  is  depicted  in  Figure  1,  where  an  Unmanned  Ground 
Vehicle  (UGV)  deploys  across  a  rocky,  sloped,  and  vegetation- covered  envi¬ 
ronment. 


< 


Figure  1:  Autonomous  driving  of  an  Unmanned  Ground  Vehicle  (UGV) 
across  an  open  terrain  with  rocks,  slopes,  and  vegetation. 

High-speed  autonomous  navigation  in  rough  terrain,  like  the  scenario  in 
Figure  1,  is  challenging  because  navigation  algorithms  must  consider  nonlin¬ 
ear  vehicle  dynamic  effects  such  as  wheel  slip,  skidding,  ballistic  behavior, 
rollover,  and  vehicle-terrain  interaction  phenomena.  Navigation  algorithms 
must  also  consider  the  presence  of  obstacles.  Additionally,  these  algorithms 
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must  be  computationally  efficient  enough  to  operate  in  real  time.  Further 
difficulties  arise  due  to  real-world  uncertainties  caused  by  unknown/time- 
varying  vehicle  parameters,  unknown/poorly  known  terrain  conditions,  and 
range  sensor  error  and  uncertainty.  The  reason  for  the  poor  understand¬ 
ing  of  mobility  in  open  terrain,  that  is,  its  ability  to  traverse  rugged  terrain 
efficiently  and  safely,  is  rooted  in  the  intrinsic  complexity  of  the  vehicle’s 
dynamics  and  in  the  vehicle/terrain  interaction.  These  properties  are  hard 
to  quantify  with  a  single  scalar  metric  involving  vehicle  and  terrain  features. 
Generally  speaking,  a  solution  to  the  motion  planning  problem  for  high-speed 
autonomous  vehicles  in  a  highly  unstructured  and  uncertain  environment  re¬ 
quires  not  only  sophisticated  vehicle  design  and  hardware  components  (e.g., 
for  actuation,  sensing,  and  communication),  but  most  importantly,  it  de¬ 
mands  advanced  software  algorithms  and  supervisory  control  strategies  that 
can  make  use  of  the  full  capabilities  of  these  components. 

The  ultimate  objective  of  this  work  is  to  design  planning  and  control  ar¬ 
chitectures  that  enable  autonomous  vehicles  to  operate  in  open  terrain  with¬ 
out  sacrificing  speed  and  maneuverability.  To  this  end,  we  develop  mobility 
metrics  for  UGVs  operating  off-road  at  high-speed  regimes,  explore  optimal 
algorithms  to  derive  optimal  paths  for  UGVs,  and  propose  a  framework  for 
motion  planning.  The  major  tasks  to  accomplish  this  goal  are  the  following: 

•  Develop  an  efficient  stochastic  terrain  traversal  prediction  tool  that 
allows  systematic  assessment  of  the  ability  of  a  vehicle  to  negotiate 
challenging  terrain.  This  tool  will  improve  the  computational  efficiency 
of  current  Monte  Carlo-based  methods  and  allow  for  the  construction 
of  a  large  library  of  obstacle  traversal  maneuvers. 

•  Design  vehicle  mobility  metrics  that  quantify  the  ability  of  a  vehicle 
to  traverse  challenging  terrain  and  to  track  trajectories  with  varying 
curvature  and  speed.  These  metrics  will  be  unique  since  they  will:  a) 
represent  a  UG  Vs  obstacle  traversal  capability  as  a  function  of  its  tra¬ 
jectory  (i.e.,  path  and  velocity),  and  b)  consider  UGV  agility  ( i.e. ,  the 
ability  to  track  high-curvature  paths,  and  execute  challenging  maneu¬ 
vers  ) . 

•  Develop  methods  for  evaluation  of  terrain/control  strategy  pairs  that 
provide  a  compact  yet  accurate  way  to  identify,  classify,  and  assess 
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terrain-specific  maneuvers.  This  task  will  bridge  the  gap  between  ac¬ 
curate  but  complex  metrics,  and  tractable  metrics  that  can  be  used  in 
real-time  path  planning. 

•  Develop  a  symbolic  control  framework  that  permits  the  execution  of 
complex,  high-performance  trajectories  through  the  on-line  combina¬ 
tion  of  a  number  of  baseline  controllers,  chosen  from  a  finite  library  of 
elementary  behaviors  or  maneuvers. 

The  results  obtained  from  a  research  period  of  one  year  are  summarized  in  the 
next  section.  A  detailed  presentation  of  these  results  as  well  as  a  thorough 
description  of  the  methodologies  employed  to  derive  them  can  be  found  in 
Appendix  A-B. 


4  Summary  of  Results 

4.1  Efficient  stochastic  terrain  traversal. 


We  have  developed  a  computationally  efficient  method  for  mobile  robot  mo¬ 
bility  prediction.  Such  a  method  would  be  useful  in  analysis  of  high  speed 
motion,  especially  in  uneven,  natural  terrain.  It  would  also  find  applica.- 
tion  in  both  motion  planning  and  control  algorithms  designed  for  high  speed 
scenarios. 

Our  approach  to  mobility  prediction  is  based  on  the  stochastic  response 
surface  method  (SRSM).  The  SRSM  is  a  method  for  efficient  representation 
of  the  response  of  systems  that  are  subject  to  uncertainty.  In  this  approach, 
we  represent  model  inputs  as  functions  of  normal  random  variables,  with  each 
having  zero  mean  and  unit  variance.  The  same  set  of  random  variables  is 
then  used  to  represent  a  user-specified  output.  An  equivalent  reduced  model 
for  the  output  is  expressed  in  the  form  of  a  series  expansion,  consisting  of 
multi-dimensional  Hermite  polynomials  of  normal  random  variables,  as: 


y  =  ao 


(M, 


n  f-i 


ri(61)  +  2^2^^r2(^’^)  + 


i=l 


1=1  Z2  =  l 


where  y  refers  to  an  output  metric,  ..  are  coefficients  to  be  deter¬ 
mined,  . . .  are  i.i.d.  normal  random  variables,  and  F^q ,  &2,  . . . ,  &  ) 

is  the  Hermite  polynomial  of  degree  q. 

The  unknown  coefficients  are  then  estimated  from  a  small  number  of 
model  simulations,  by  choosing  a  set  of  sample  points  (i.e.  collocation  points), 
calculating  the  model  output  at  these  points,  then  applying  regression  to  find 
the  coefficients.  The  approximate  reduced  model  was  then  used  to  analyze 
the  system  subject  to  uncertainty.  It  has  been  shown  that  this  approach 
yields  results  that  are  comparable  in  accuracy  to  the  outputs  of  classical 
Monte  Carlo  approaches. 

The  SRSM  approach  can  be  applied  to  a  variety  of  applications  such 
as  those  related  to  mobility  prediction  (including  obstacle  traversal,  slope 
traversal,  and  rollover  analysis)  and  motion  planning  (i.e.  terrain  dependent 
path  generation)  while  explicitly  considering  uncertainty  in  terrain  and/or 
vehicle  parameters.  The  form  of  the  uncertainty  distribution  can  be  specified, 
and  can  be  state  or  position  dependent. 

For  mobility  analysis  of  a  vehicle  traversing  sloped,  deformable  terrain,  a 
simple  description  of  mobility  was  defined  as  the  probability  that  for  a  given 
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initial  velocity  at  an  initial  position,  the  robot  will  have  a  non-negative  ve¬ 
locity  after  moving  up  the  incline.  Taking  into  account  uncertainty  in  terrain 
parameters,  this  was  presented  as  a  distribution  of  traversal  probability  ver¬ 
sus  initial  velocity,  which  could  then  be  used  to  predict  for  which  velocities 
the  robot  will  be  able  to  traverse  the  deformable  terrain  region  with  a  reason¬ 
ably  high  probability.  A  classical  Bekker-type  wheel  soil  interaction  model 
was  used  to  calculate  the  drawbar  pull  (i.e.  net  longitudinal  wheel  thrust). 
An  equivalent  model  for  the  drawbar  pull  was  then  formulated  using  the 
SRSM  approach  which  was  then  used  to  calculate  the  traversal  probabilities. 

The  approach  was  also  used  for  mobility  analysis  of  a  vehicle  traveling 
along  a  side  slope.  A  probabilistic  reachability  metric  was  generated  based  on 
the  statistics  of  t  he  (spatial)  distribution  of  trajectories  resulting  from  a  given 
open  loop  input  over  various  distinct  terrain  types,  and  the  mean  trajectory 
was  plotted  along  with  the  probability  ellipsoids;  the  latter  representing  the 
deviation  from  the  mean  path  under  uncertainty.  This  is  shown  in  Figure  2. 


Figure  2:  Mobility  analysis  “Reachability  Metric”  for  vehicle  traveling  along 
a  side-slope. 

The  SRSM  method  was  also  applied  to  rollover  analysis  of  a  Dubins  ve¬ 
hicle  for  various  steering  high  speed  maneuvers.  The  rollover  tendency  was 
analyzed  while  considering  uncertainty  in  the  vehicle  roll  stiffness  parame- 
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ters.  The  linear  model  considered  included  lateral  acceleration,  vaw  and  roll 
dynamics.  It  was  shown  that,  as  expected,  explicit  consideration  of  uncer¬ 
tainty  yielded  a  richer  description  of  the  rollover  probability  as  compared  to 
a  deterministic  model-based  approach. 

Finally,  initial  investigation  into  integrating  the  SRSM  approach  with  a 
motion  planning  method  was  performed.  The  motion  planning  method  was 
based  on  rapidly  exploring  random  trees  (RRTs).  Here,  the  SRSM  method 
was  used  to  calculate  mobility-related  statistics  for  each  branch  of  the  motion 
plan  tree  for  a  high  speed  vehicle  motion.  The  resulting  motion  plan  therefore 
implicitly  considered  uncertainty  in  its  determination  of  an  safe,  rapid  path 
across  uneven  terrain. 

Our  major  results  are  as  follows: 

•  A  method  for  mobile  robot  mobility  prediction  based  on  the  stochastic 
response  surface  method  was  developed.  Results  obtained  using  the 
SRSM  matched  closely  with  those  obtained  through  the  use  of  the 
Monte  Carlo  methods.  The  following  advantages  were  observed: 

—  Computational  time  for  the  SRSM  was  found  to  be  approximately 
two  orders  of  magnitude  lower  than  that  of  classical  Monte  Carlo 
methods. 

—  The  SRSM  approach  allows  for  the  explicit  consideration  of  un¬ 
certainty  (in  vehicle  and/or  terrain  models)  during  aggressive  ma¬ 
neuvering. 

—  The  approach  represents  a  potential  pathway  towards  robust  ag¬ 
gressive  control  framework. 

*  The  SRSM  method  was  applied  to  several  practical  application  sce¬ 
narios,  including  mobility  prediction  on  sloped  terrain,  and  high  speed 
motion  planning.  Major  results  included: 


—  Simulation  results  of  an  analysis  of  vehicle  mobility  on  a  side  slope. 

—  Initial  results  for  a  motion  planning  method  that  explicitly  con¬ 
siders  model  uncertainty. 

Appendix  B  contains  a  conference  paper  that  is  a  direct  result  of  this 
research. 
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4.2  Optimal  motion  planning  on  heterogeneous  ter¬ 
rain. 

We  consider  the  problem  of  computing  optimal  paths  for  UGVs  from  a  given 
configuration  to  a  final  configuration.  For  simplicity,  we  assumed  that  the 
environment  is  obstacle-free  and  a  simple  kinematic  model  of  the  car  depicted 
in  Figure  3,  which  is  given  by  the  equations 

x  =  v  cos  0, 

y  =  i;sin(?,  (1) 

0  =  u, 

where  (x,y,9)  e  SE(2),  (x,y)  is  the  vehicle’s  position,  6  is  the  angle  between 
the  vehicle  and  the  vertical  axis  determining  the  vehicle’s  orientation,  v  is  the 
forward,  positive  velocity  and  u  is  the  bounded  angular  acceleration  input, 
which,  without  loss  of  generality,  is  assumed  to  take  value  in  [—1,1].  This 
vehicle  model  is  usually  referred  to  as  Dubins  vehicle  and  has  been  broadly 
used  as  a  kinematic  model  for  path  planning  of  UGVs  (and  UAVs),  like  the 
one  depicted  in  Figure  3. 


Figure  3:  Dubins  car. 

For  a  UGV  given  by  (1)  on  an  open  terrain,  our  goal  is  to  compute 
the  optimal  path  starting  at  a  given  initial  configuration  and  ending  at  a 
given  final  configuration.  The  parameters  in  the  model  (1)  vary  for  terrains 
with  different  properties.  The  maximum  forward  speed  changes  with  the 
terrain  roughness  while  the  maximum  curvature  of  the  paths  depends  on 
the  friction  coefficient  of  the  terrain.  A  convenient  modeling  abstraction  for 
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navigation  on  heterogeneous  open  terrain  is  to  classify  terrain  regions  based 
on  these  parameters  and  then  associate  to  each  region  a  vehicle  model  with 
appropriate  maximum  forward  velocity  and  friction  coefficient.  For  clarity 
in  the  exposition,  we  only  consider  the  case  of  heterogeneous  velocity  along 
the  terrain  where  the  vehicle  is  deployed.  Two  different  velocities,  and  v2, 
define  the  constant,  forward  velocity  of  Dubins  vehicle  on  two  patches  of  the 
plane,  patch  V\  and  patch  V2,  depicted  in  Figure  4.  We  are  interested  in 
solving  the  following  problem: 

Find  the  minimum-time  path  for  Dubins  vehicle  from  an  initial 

configuration  in  patch  Vi  to  a  final  configuration  in  patch  V2. 

Figure  4  shows  possible  initial  and  final  vehicle  configurations,  which  are 
denoted  by  (x°,y°,60)  and  (z1,?/1,#1),  respectively,  for  which  a  minimum¬ 
time  path  is  to  be  found.  To  the  best  of  our  knowledge,  the  problem  described 
above  has  not  been  addressed  in  the  past,  perhaps  due  to  the  fact  that  the 
classical  Pontryagin’s  Maximum  Principle  is  not  applicable  because  of  the 
discontinuous  behavior  at  the  common  boundary  between  the  patches. 


Figure  4:  Dubins  vehicle  on  an  heterogeneous  terrain.  The  initial  configu¬ 
ration  is  given  by  (x°,y°,  9°)  and  the  final  configuration  by  (rc1,?/1,#1).  The 
forward  velocity  in  patch  V\  is  smaller  than  the  forward  velocity  in  patch  V2. 

We  have  established  conditions  for  time-optimal  maneuvers  of  autonomous 
vehicles  operating  on  terrain  with  variable  characteristics.  Our  results  include 
conditions  that  the  paths  need  to  satisfy  at  the  boundary  between  terrains 
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with  different  maximum  velocity.  These  conditions  reduce  notably  the  com¬ 
putational  load  in  selecting  maneuvers  and  enable  the  generation  of  optimal 
motion  planning  algorithms.  We  summarize  them  as  follows: 

•  The  portions  of  the  paths  that  remain  in  either  region  are  Dubins  op¬ 
timal. 

•  Optimal  paths  are  such  that,  at  the  boundary  between  the  regions, 
their  type  does  not  change. 

•  Optimal  paths  that  cross  the  boundary  describing  a  straight  line  are 
orthogonal  to  the  boundary. 

•  The  angles  of  the  path  pieces  within  each  region  before  and  after  cross¬ 
ing  the  boundary  satisfy  a  refraction  law. 

Appendix  B  contains  formal  statements  of  these  results. 

4.3  Robust  control  algorithms  for  motion  primitive  se¬ 
lection. 

Given  a  mathematical  model  of  a  vehicle,  or  in  more  general  terms,  a  dy¬ 
namical  system,  with  state  x  and  input  u  (e.g.  a  static  input/output  map, 
an  ordinary  differential  equation  with  inputs,  a  partial  differential  equation 
with  inputs,  etc.)  describing  its  behavior  for  each  given  input,  the  motion 
planning  problem  for  is  that  of  given  a  motion  plan  v,  find  a  control  input 
u  such  that  the  state  x  satisfies  the  specification  given  by  the  motion  plan 
v.  to  r.  This  specification  is  usually  given  in  terms  of  a  a  curve  in  the  state 
space  parameterized  by  time  f,  in  which  case  the  goal  of  a  motion  planning 
algorithm  is  to  steer  x  to  that  curve.  Algorithms  that  accomplish  such  a 
task  are  commonly  applied  in  robotics  as  a  method  to  solve  steering  prob¬ 
lems.  One  challenge  for  these  algorithms  is  that  in  real-world  settings,  they 
must  accomplish  the  motion  planning  task  in  the  presence  of  obstacles,  mea¬ 
surement  error,  exogenous  disturbances,  and  unmodeled  dynamics.  Hence, 
reactivity  and  robustness  are  highly  desired  properties  for  these  algorithms. 

A  particular  class  of  motion  planning  algorithms,  which  are  known  as 
maneuver-based  motion  planning  algorithms,  exploits  the  symmetry  proper¬ 
ties  present  in  certain  classes  of  nonlinear  systems,  in  particular,  UGVs,  to 
perform  challenging  motions.  Motion  primitives  available  in  a  pre-defined  li¬ 
brary,  designed  off-line  with  model-based  design  tools,  trial  and  error,  and/or 


obtained  from  motions  generated  by  humans,  are  concatenated  to  perform 
a  given  motion  plan.  For  example,  an  UGV  can  be  controlled  to  perform 
motions,  like  the  one  in  Figure  5,  by  breaking  the  motion  in  “pieces”  where 
inputs  are  constants,  referred  to  as  trim  trajectories,  and  where  the  inputs  are 
varying  with  certain  law,  referred  to  as  maneuvers.  Figure  5  denotes  the  dif¬ 
ferent  pieces.  Typically  in  maneuver-based  motion  planners,  trim  trajectories 
and  maneuvers  are  executed  by  applying  appropriate  open-loop  control  laws. 
A  method  to  synchronize  the  control  decisions  is  to  implement  a  switching 
logic  in  a  maneuver  automaton. 


Figure  5:  Motion  plan  for  an  UGV  divided  into  trim  and  maneuver  pieces. 

The  open-loop  nature  of  the  maneuver-based  motion  planning  method 
outline  above  limits  its  application  to  nominal  scenarios,  that  is,  those  with¬ 
out  perturbations,  e.g.  measurement  noise,  unmodeled  dynamics,  etc.,  which, 
in  turn,  narrows  its  applicability  to  steering  of  vehicles  across  open  terrain. 
Even  for  smooth  feedback  systems,  the  presence  of  arbitrarily  small  pertur¬ 
bations  can  lead  to  totally  different  behavior  than  in  the  nominal  case.  In 
fact,  establishing  robustness  (vaguely,  the  property  that  under  arbitrarily 
small  perturbations  the  system  solutions  are  “close”  to  the  nominal  ones) 
is  not  a  straightforward  task,  even  in  scenarios  with  homogeneous  terrain. 
Moreover,  the  metric  used  to  characterize  closeness  between  solutions  trajec¬ 
tories  (for  example,  the  distance  between  a  trim  reference  trajectory  and  a 
UGV  trajectory)  should  take  into  account  that  corresponding  pieces  in  the 
nominal  and  perturbed  motions  may  occur  over  different  time  intervals. 

Another  challenge  in  robustifying  maneuver-based  motion  planning  is 
that  the  nominal  trajectories  resulting  from  it  are  not  always  necessarily 
smooth.  Hence,  standard  trajectory  tracking  control  design  techniques  are 
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not  applicable.  Even  when  these  standard  tools  are  applicable,  tracking  de¬ 
sign  for  aggressive  maneuvers  is  not  always  an  easy  task  (for  example,  to 
design  a  tracking  controller  for  an  UGV  to  track  motion  plans  having  tight 
turn  maneuvers  would  require  a  control  law  that  guarantees  fast  tracking  at 
the  turns  while  at  other  pieces  of  the  reference  trajectory,  slower  tracking 
would  suffice). 

To  address  some  of  the  issues  mentioned  above,  we  propose  a  hybrid 
control  framework  for  robust  maneuver-based  motion  planning.  The  major 
results  are  as  follows: 

•  Trim  trajectories  and  ■ maneuvers  can  be  combined  in  a  hybrid  controller 
to  execute  maneuver-based  motion  plans  by  means  of  a  timer ,  two  logic 
variables,  and  one  auxiliary  state. 

•  The  graphical  distance  between  trajectories  specified  by  the  motion  plan¬ 
ner  and  the  trajectories  to  the  closed-loop  ( hybrid )  system  is  an  appro¬ 
priate  metric  to  evaluate  closeness  between  motions. 

•  Robustness  of  tracking  of  maneuver-based  motion  planning  for  general 
nonlinear  systems  with  symmetries  to  perturbations  in  the  initial  con¬ 
ditions,  external  disturbances ,  and  unmodeled  dynamics. 

We  show  that  this  framework  results  in  a  hybrid  system  with  implementable 
semantics,  and  hence,  useful  experimental  setups.  The  resulting  transition 
system  and  control  methodology  is  such  that,  given  a  sequence  of  commands, 
selects  and  executes  a  particular  maneuver  from  a  library  of  motion  primi¬ 
tives.  The  main  feature  of  the  system  is  its  robustness  to  external  perturba¬ 
tions,  which  are  typical  in  controlling  UGVs  in  challenging  terrain  and,  unless 
the  associated  control  algorithm  is  robust,  concatenation  of  motion  primitives 
would  not  be  successful.  More  details  can  be  found  in  Appendix  C. 

Regarding  the  generation  of  motion  primitives,  which  is  key  in  the  con¬ 
struction  of  a  library  of  trim  trajectories  and  maneuvers  for  the  purposes  of 
motion  planning,  we  provide  a  detailed  calculation  of  trim  trajectories  for  a 
particular  UGV  model:  the  half  car.  We  show  that  its  set  of  trim  trajectories 
includes  trajectories  at  the  limit  of  the  vehicle’s  performance,  like  those  as¬ 
sociated  to  vehicle  motions  under  slipping  and  skidding  conditions  occurring 
at  large  velocity  and  small  turning  radius,  which  are  typical  in  open  terrain 
settings.  See  Appendix  I)  for  more  details. 
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In  Uncertain  Environments 
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ABSTRACT 

The  ability  of  autonomous  unmanned  ground  vehicles  (UGVs)  to  rapidly  and  effectively  predict  terrain  negotiability  is  a 
critical  requirement  for  their  use  on  challenging  terrain.  Most  methods  for  assessing  traversability,  however,  assume 
precise  knowledge  of  vehicle  and  terrain  properties.  In  practical  applications,  uncertainties  are  associated  with  the 
estimation  of  the  vehicle/terrain  parameters,  and  these  uncertainties  must  be  considered  while  determining  vehicular 
mobility.  Here  a  computationally  inexpensive  method  for  efficient  mobility  prediction  based  on  the  stochastic  response 
surface  (SRSM)  approach  is  presented  that  considers  imprecise  knowledge  of  terrain  and  vehicle  parameters  while 
analyzing  various  metrics  associated  with  UGV  mobility.  A  conventional  Monte  Carlo  method  and  the  proposed 
response  surface  methodology  have  been  applied  to  two  simulated  cases  of  mobility  analysis,  and  it  has  been  shown  that 
the  SRSM  method  is  an  efficient  tool  as  compared  to  conventional  Monte  Carlo  methods  for  the  analysis  of  vehicular 
mobility  in  uncertain  environments. 

Keywords:  Homogeneous  Chaos,  Monte  Carlo,  Latin  Hypercube  Sampling,  Vehicle  Mobility,  Vehicle  Rollover, 
Stochastic  Response  Surface,  Terrain  Modeling,  Unmanned  Ground  Vehicle,  Wheel-Soil  Interaction 


1.  INTRODUCTION 

Future  Army  operations  will  employ  autonomous  or  semi-autonomous  unmanned  ground  vehicles  (UGVs)  in  both  cross¬ 
country  and  urban  environments.  A  fundamental  requirement  of  these  vehicles  is  to  quickly  predict  their  ability  to 
negotiate  rough  terrain  regions  and  surmount  obstacles.  This  mobility  prediction  capability  is  critical  to  the  successful 
deployment  of  UGVs  that  can  operate  effectively  on  challenging  terrain  with  minimal  human  supervision. 

Significant  work  has  been  done  to  understand  and  predict  the  mobility  of  vehicles  in  natural  terrain  [1],  [2].  However, 
these  efforts  assume  accurate  knowledge  of  vehicle  parameters  and  wheel-  (or  track-)  soil  interaction  properties, 
gathered  from  terrain  measurement  devices  such  as  cone  penetrometers.  In  field  conditions,  however,  UGVs  often  only 
have  access  to  sparse  and  uncertain  parameter  estimates  drawn  from  “standard”  robotic  sensors  such  as  LIDAR. 
Moreover,  significant  uncertainties  are  often  associated  with  estimates  of  vehicle  parameters,  due  to  effects  such  as 
loading,  wear,  fuel  consumption,  etc.  It  is  thus  critical  to  consider  these  uncertainties  when  deriving  predictions  of 
vehicle  mobility. 

There  exists  a  vast  body  of  literature  on  techniques  to  estimate  the  probability  distributions  of  processes  that  are  subject 
to  uncertainty.  Such  techniques  could  be  applied  to  the  mobility  prediction  problem,  by  first  modeling  the  uncertainty  in 
vehicle  and  terrain  parameters,  then  defining  a  range  for  their  probable  values,  and  finally  analyzing  the  performance  of 
a  UGV  model  over  that  parameter  space,  as  in  [3].  The  result  would  be  a  prediction  of  the  ability  of  a  UGV  to 
successfully  traverse  a  given  route  that  rigorously  considers  vehicle  and  terrain  parameter  uncertainty.  This  analysis  can 
be  performed  using  a  variety  of  techniques  such  as  interval  mathematics,  probabilistic  methods  and  fuzzy  set  theory, 
among  others  [4],  [5]. 

A  traditional  method  for  estimating  the  probability  density  function  of  a  system’s  output  response  from  known  or 
estimated  input  distributions  is  the  Monte  Carlo  method  [6],  [7],  This  approach  involves  the  random  selection  of  a  value 
for  each  uncertain  parameter  from  its  uncertainty  range,  weighted  by  its  probability  of  occurrence,  followed  by  model 
simulation  using  this  parameter  set.  This  process  is  repeated  many  times  to  obtain  the  probability  distribution  of  an 
output  metric. 
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Since  parameter  values  are  selected  randomly,  a  large  number  of  simulation  runs  is  often  required  to  obtain  reasonable 
results,  leading  to  a  (usually)  high  computational  cost.  Structured  sampling  techniques  such  as  Latin  hypercube  sampling, 
importance  sampling,  and  others  can  be  used  to  improve  computational  efficiency,  however  these  gains  may  be  modest 
for  complex  problems  [8],  [9], 

More  recent  approaches  to  stochastic  simulation  include  the  polynomial  chaos  approach,  which  is  based  on  Wiener’s 
theoiy  of  homogeneous  chaos.  Since  the  introduction  of  the  spectral  stochastic  finite  element  method  [10],  polynomial 
chaos  has  been  successfully  applied  to  represent  uncertainty  in  various  structural  and  fluid  mechanics  problems. 
Recently,  researchers  have  applied  this  technique  to  the  dynamic  simulation  of  a  7  DOF  vehicle  [11].  However,  the 
collocation  approach  employed  therein  has  been  noted  to  be  inherently  unstable  and  exhibit  convergence  problems  [12]. 
Moreover,  different  combinations  of  collocation  points  may  lead  to  considerably  different  output  estimates,  or  they  may 
not  correspond  to  high  probability  regions  of  the  input  parameter  space. 

Here  we  propose  the  use  of  the  stochastic  response  surface  method  (SRSM),  as  described  by  Isukapalli  [13],  [14]  for  the 
mobility  prediction  of  UGVs  in  natural  terrain  that  uses  a  regression  based  approach  to  obtain  an  equivalent  reduced 
model  for  the  output  and  serves  as  a  computationally  inexpensive  tool  for  predicting  the  traversability  of  a  UGV  over 
rugged  terrain. 

This  paper  is  organized  as  follows.  In  Section  2,  we  briefly  introduce  the  Monte  Carlo  and  SRSM  methods  and  present 
their  application  to  vehicle  dynamic  modeling.  This  is  followed  by  a  description  of  a  three  degree  of  freedom  vehicle 
model  in  Section  3.  The  effect  of  terrain  physical  parameter  uncertainty  on  vehicle  mobility  is  analyzed.  Simulation 
results  obtained  using  Monte  Carlo  and  SRSM  approaches  are  compared  in  Section  4.  It  can  be  seen  that  accurate, 
efficient  statistical  mobility  prediction  can  be  achieved  using  the  proposed  response  surface  techniques. 

2.  UNCERTAINTY  ANALYSIS  TECHNIQUES 


2.1  Monte  Carlo  Method 

With  the  advancements  in  computational  technology,  Monte  Carlo  techniques  have  found  increasing  application  in 
numerous  fields  over  the  last  several  years.  These  methods  typically  involve  a  (usually)  large  number  of  simulation  runs 
of  an  analytical  or  numerical  system  model  using  various  combinations  of  model  parameters,  followed  by  the  subsequent 
analysis  of  the  outputs.  In  other  words,  the  model  parameters  (known  as  “input  parameters”)  are  randomly  sampled  from 
their  respective  probability  distributions,  which  are  assumed  to  be  known  (or  estimated)  a  priori,  and  multiple  simulation 
runs  are  conducted  using  each  set  of  the  input  parameter  values  to  obtain  the  corresponding  outputs  for  each  case.  En 
estimate  of  the  probability  distribution  of  a  user-defined  output  metric  can  then  be  estimated. 

A  variety  of  methods  have  been  developed  for  efficient  sampling  from  input  parameter  probability  distributions, 
including  (among  others)  stratified,  importance  and  Latin  Hypercube  sampling  [15],  [16].  Generally,  these  methods 
focus  on  ensuring  that  samples  are  generated  from  the  entire  range  of  the  input  parameter  space  while  reducing 
computational  costs,  and  are  thus  an  improvement  over  the  standard  Monte  Carlo  method. 

In  the  mobility  prediction  scenario,  vehicle  and  terrain  parameters  are  designated  as  uncertain  input  parameters.  A 
fundamental  assumption  of  the  proposed  approach  is  that  while  the  terrain  and/or  vehicle  parameters  may  not  be 
precisely  known,  engineering  estimates  of  their  distributions  are  available.  This  is  a  reasonable  assumption  for  UGV 
physical  parameter  estimates,  since  the  effects  of  loading,  component  wear,  and  parameter  uncertainty  can  generally  be 
bounded  with  reasonable  accuracy.  It  is  also  a  reasonable  assumption  for  terrain  parameter  estimates,  since  many 
methods  exist  for  coarsely  classifying  terrain  from  standard  robotic  sensors  such  as  LIDAR  and  vision  [17]-[1 9], 

2.1.1  Algorithmic  implementation 

Here  we  discuss  the  general  Monte  Carlo  approach  as  applied  to  mobility  analysis.  The  method  considers  functions  of 
the  form: 


Y  =  g(X) 


a) 


where  g  represents  the  model  under  consideration,  X  is  a  vector  of  uncertain  input  variables  and  Y  represents  a  vector  of 
estimated  outputs. 


A  general  procedure  for  the  analysis  is  as  follows: 

a)  Construct  a  vector  X  consisting  of  n  relevant  terrain  and/or  vehicle  parameters.  To  define  the  input  parameter 
space  and  to  characterize  the  uncertainty  in  the  elements  of  X,  assign  a  probability  distribution  to  each  input 
parameter,  based  on  corresponding  engineering  estimates. 

While  many  forms  of  the  input  parameter  distribution  are  possible,  in  this  paper,  the  parameter  values  are  assumed 
to  have  a  Gaussian  distribution  and  to  be  uncorrelated. 

b)  Generate  a  sample  value  for  each  of  the  n  input  variables  from  the  corresponding  probability  distribution.  More 
specifically,  a  sample  set: 

Xl=[*fl’XJ2 . **]  (T) 

is  generated  from  the  input  parameter  space.  This  set  may  be  generated  randomly  or  using  the  structured  sampling 
techniques  such  as  stratified  sampling,  importance  sampling  or  Latin  Hypercube  sampling. 

In  the  '"standard”  Monte  Carlo  approach,  random  sampling  of  the  input  parameter  distributions  is  performed. 
However,  to  ensure  representation  of  the  entire  parameter  range,  a  large  number  of  simulations  must  often  be 
performed.  Stratified  sampling,  on  the  other  hand,  partitions  the  sample  space  into  a  number  of  strata,  with  each 
stratum  having  a  specified  probability  of  occurrence.  Random  samples  are  then  drawn  from  each  stratum.  While  this 
ensures  dense  coverage  of  the  parameter  space,  the  definition  of  the  strata  and  the  calculation  of  their  probabilities 
must  be  carefully  addressed.  Latin  hyper  cube  sampling  can  ensure  dense  coverage  of  the  range  of  each  input 
variable  while  avoiding  the  difficulties  associated  with  stratified  sampling.  This  is  achieved  by  dividing  each  input 
parameter’s  range  into  disjoint  intervals  of  equal  probability  and  then  randomly  sampling  a  parameter  value  from 
each  interval.  This  is  illustrated  in  Figure  1 . 
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Fig.  1 .  Illustration  of  sampling  using  the  Latin  hypercube  method. 


c)  Evaluate  the  output  response  from  the  system  model  under  analysis  using  the  values  from  the  input  parameter  set 
Xj  as  model  parameter  values. 


d)  Repeat  steps  b)  and  c)  to  generate  a  probability  distribution  for  the  output  metric.  Various  statistics  such  as  the 
estimated  expectation,  p,  or  variance,  can  then  be  determined  as  follows: 

1  " 


A  =  ^2XXj) 

j=l 


(3) 


H 


(4) 


The  number  of  simulations  (TV)  is  chosen  to  be  large  enough  such  that  the  output  distribution  converges  to  a  stable  value. 


Figure  2  below  represents  schematically  the  general  Monte  Carlo  approach  For  uncertainty  analysis: 
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Fig.  2,  Illustration  ot' uncertainty  analysis  using  the  Monte  Carlo  method. 

2*2  Stochastic  Response  Surface  Method 

The  stochastic  response  surface  method  (SRSM)  represents  inputs  and  outputs  of  a  system  under  consideration  via  series 
approximations  using  standard  random  variables,  which  results  in  a  computationally  efficient  means  for  uncertainty 
propagation  through  the  models.  In  SRSM,  inputs  arc  represented  as  functions  of  normal  random  variables,  each  having 
zero  mean  and  unit  variance.  The  same  set  of  random  variables  that  is  used  to  represent  input  stochastieity  can  then  be 
used  for  representation  of  outputs. 

An  equivalent  reduced  model  for  an  output  is  expressed  in  the  form  of  a  scries  expansion  consisting  of  multi¬ 
dimensional  Hcnnite  polynomials  of  normal  random  variables,  as: 


>=«0+I>,W+]r  i>„r2(£4)+... 


(5) 


where  y  refers  to  an  output  metric,  <7(7,  af2,.  .  are  coefficients  to  be  determined,  £ih  £7,.,.  are  i.i.d.  normal  random 
variables,  and  Fq(^i,  ^q)  *s  the  Hcnnite  polynomial  of  degree  q s  given  as: 
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For  notational  simplicity,  the  scries  may  be  written  as: 

>=0 


(7) 


where  the  series  is  truncated  to  a  finite  number  of  terms  and  there  exists  a  correspondence  between  rq(£/T  . £<?)  and 

<t>(£),  and  their  corresponding  coefficients. 

The  series  expansion  contains  unknown  coefficient  values  that  can  be  estimated  from  a  limited  number  of  model 
simulations  to  generate  an  approximate  reduced  model  This  is  achieved  by  choosing  a  set  of  sample  points  from  high 
probability  regions  then  calculating  the  model  output  at  these  points  [13],  [20],  A  regression  based  approach  is  then 
utilized  to  obtain  values  for  the  unknown  coefficients  [14],  Once  the  (statistically  equivalent)  reduced  model  is 
formulated,  it  can  be  used  to  facilitate  analysis  of  the  system  subject  to  uncertainty'. 

This  procedure  thus  results  in  a  reduction  in  the  number  of  model  simulations  (and,  therefore,  a  reduction  in 
computational  cost)  required  for  estimation  of  output  uncertainty  ,  as  compared  to  the  conventional  probabilistic  methods 
such  as  Monte  Carlo  methods.  Further  details  on  SRSM  can  be  found  in  [14]. 

2,2*1  Algorithmic  implementation 

Here  a  summary  of  the  SRSM  method  is  presented  as  applied  to  robotic  mobility  prediction. 


a)  Represent  uncertain  input  parameters  in  terms  of  standard  random  variables  (here  Gaussian  variables).  A 
terrain/vehicle  parameter^  can  be  written  as: 

Xj  =  ftj  +  <TjZ  (8) 


where  pi}  is  the  mean,  oj  represents  the  standard  deviation  and  £  is  a  standard  normal  random  variable. 

b)  Express  the  model  output  under  consideration  in  terms  of  the  same  set  of  random  variables.  While  for  Gaussian 
variables,  Hermite  polynomials  are  used,  different  orthogonal  polynomial  basis  functions  are  used  corresponding  to 
the  probability  distributions  of  other  non-Gaussian  variables.  This  is  shown  in  Table  1 . 


c)  Estimate  the  unknown  coefficients  of  the  approximating  series  expansion.  This  is  accomplished  via  a  regression 
based  approach,  first  by  computing  the  model  output  at  a  set  of  collocation  points  [13],  [20],  These  points  are 
selected  such  that  each  standard  random  variable  takes  a  value  of  either  zero  or  a  root  of  the  Hermite  polynomial  of 
a  higher  order.  This  ensures  that  points  from  high  probability  regions  are  represented.  Taking  the  number  of 
collocation  points  (M)  to  be  nearly  twice  in  number  to  the  number  of  coefficients  (M+l )  has  been  shown  to  yield 
robust  coefficient  estimates  [1 4],  [20],  Calculation  of  the  model  output  at  these  points  results  in  set  of  equations  with 
the  number  of  equations  exceeding  the  number  of  unknown  coefficients.  Using  the  linear  least  square  method  and 
singular  value  decomposition,  the  system  of  linear  equations  similar  to  the  one  shown  below  can  be  solved: 


'r0(£)  ^(4)  ....  rw(4)V^0(O]  (y&^Y 

r0(4)  ^(4)  ....  rw(4)  *(/)  y(/,4) 
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(9) 


The  reduced  equivalent  model  can  henceforth  be  used  for  the  analysis,  which  avoids  the  requirement  of  multiple  runs  of 
the  (generally)  non-linear  model,  thus  resulting  in  reduced  simulation  time.  The  advantage  of  the  SRSM  technique  is 
therefore  that  the  number  of  model  simulations  is  greatly  reduced  relative  to  conventional  methods,  thus  improving 
computational  efficiency.  Further,  the  accuracy  of  the  computational  model  can  often  be  increased  by  increasing  the 
order  of  the  polynomial  chaos  expansion. 


3.  3.  MOBILITY  PREDICTION  SCENARIO 

3.1  Traversal  over  Uncertain  Terrain 

Here  an  analysis  of  a  simplified  mobile  robot  terrain  traversal  scenario  is  presented  using  the  SRSM  technique,  which 
considers  a  mobile  robot  traveling  on  flat  outdoor  terrain  (here  modeled  as  heavy  clay)  and  then  attempting  to  navigate 
up  an  inclined  region  of  highly  deformable  terrain  (here  modeled  as  diy  sand).  This  is  illustrated  in  Figure  3.  It  is 
assumed  that  significant  uncertainty  is  associated  with  a  small  number  of  critical  terrain  physical  parameters  (here, 
cohesion  and  internal  friction  angle).  The  UGV’s  mobility  is  analyzed  using  a  baseline  “standard”  Monte  Carlo  approach 
(SMC),  a  Latin  hypercube  Monte  Carlo  approach  (LHSMC)  and  the  stochastic  response  surface  technique. 

A  simple  description  of  mobility  in  the  proposed  scenario  is  defined  as  the  probability  that  for  a  given  initial  velocity 
(u0)  at  the  initial  position  (A)  (see  Figure  3),  the  robot  will  have  a  non-negative  velocity  at  point  (B),  after  moving  up  the 
sandy  incline.  Taking  into  account  the  uncertainty  in  terrain  parameters,  this  can  be  presented  as  a  distribution  of 
traversal  probability  versus  initial  velocity,  which  can  then  be  used  to  predict  for  which  velocities  the  robot  will  be  able 
to  traverse  the  deformable  terrain  region  with  a  reasonably  high  probability. 


Fig.  3 .  Simplified  scenario  considered  for  mobility  prediction  under  uncertainty 

A  classical  Bekker-type  wheel  soil  interaction  model  is  used  to  calculate  the  drawbar  pull  (i.e.  net  longitudinal  wheel 
thrust)  for  the  above  analysis  [21],  [22].  This  model  assumes  quasi -static  motion,  and  that  the  robot  wheel  is  rigid 
relative  to  the  terrain.  An  equivalent  model  for  the  drawbar  pull  is  then  formulated  using  the  approach,  which  is  later 
used  to  calculate  the  traversal  probabilities. 


w 


Fig.  4.  Wheel-terrain  interaction  model  for  rigid  wheel  on  deformable  terrain. 
For  the  vehicle  terrain  interaction  model  shown  in  Figure  4,  the  drawbar  pull  is  given  by: 

|  z{9)  cos  0  d6  -  J  cr(0)  sin  0  dO 


DP  =  rb 


(10) 


where  r (6)  and  a( 9)  represent,  respectively,  the  shear  stress  and  normal  stress  at  the  wheel-terrain  interface  (divided  into 
two  regions  in  Figure  4  to  more  clearly  represent  the  stress  distribution),  and  are  given  by: 


t(0)=  (c  +  cr(0)tan^) 


oi  (<?)  =  (-f+K)  (r(cos<9- cos  6!))” 
b 


^{0)  =  {\  +  k,)  r  (cos^-g^^j— cos6[) 


(11) 

(12) 

(13) 


The  drawbar  pull  can  hence  be  written  as: 


DP  =  rb 


A  ^1 

|  r2(<9)cos<9  dO+  J  t{(0) cos 0  dO 

0  9m 

S*  61 

-  J  cr2(0)sin  0  dO-  J  oj (0) sin  6  dO 


(14) 


The  parameters  employed  in  (10)-(14)  are  given  in  Table  2. 


Table  2.  Parameters  Involved  In  Drawbar  Pull  Calculation 


Symbol 

Quantity 

r 

Wheel  radius 

b 

Wheel  width 

6i 

Angle  corresponding  to  start  of  contact 

o2 

Angle  corresponding  to  loss  of  contact 

@m 

Maximum  stress  angle 

C 

Cohesion 

9 

Internal  friction  angle 

i 

Wheel  slip 

n 

Sinkage  exponent 

K  K 

Pressure  sinkage  moduli 

Governing  equations  of  motion  for  the  mobility  prediction  scenario  can  now  be  written  as: 


xdx  =  xchc 

(15) 

..  DP 

(16) 

x  = - -  gsirn// 

m 


where  m  is  the  vehicle  mass,  g  represents  the  acceleration  due  to  gravity  and  y  is  the  angle  of  the  incline  w.r.t.  the 
horizontal. 


3.1.1  Application  of  SRSM 

As  part  of  this  approach,  a  reduced  stochastic  model  is  developed  for  drawbar  pull  (DP)  considering  c  and  (p  as  uncertain 
parameters  with  normal  distributions.  Cohesion  and  internal  friction  angle  parameters  are  represented  as: 


C  =  Pc  +  &c 

where  and  £  are  standard  normal  random  variables.  Drawbar  pull  is  now  expressed  as: 

DP  =  a0+  a£c  +  a2^  +  a3  (£2  - 1)  +  a4  - 1)  + 


(17) 

(18) 

(19) 


The  parameters  c  and  <p  were  chosen  since  they  exhibit  significant  influence  on  DP.  Although  they  are  assumed  to  be 
normally  distributed,  other  possible  probability  distributions  (such  as  uniform  or  beta  distribution)  can  be  considered  as 
well.  The  corresponding  values  for  c  and  <p  used  in  this  analysis  can  be  found  in  Table  3. 

Table  3.  Probability  Distribution  Information  For  Uncertain  Terrain  Parameters  (c,  <p) 


Parameter 

Distribution 

Function 

Mean 

Std.  Dev. 

c  (Heavy  Clay) 

Gaussian 

69  kPa 

8.50  kPa 

(p  (Heavy  Clay) 

Gaussian 

34  deg 

2.10  deg 

c  (Dry  Sand) 

Gaussian 

1.04  kPa 

0.125  kPa 

<p  (Dry  Sand) 

Gaussian 

28  deg 

1.75  deg 

3.2  Rollover  Analysis  considering  Vehicle  Parameter  Uncertainty 

Here  a  three  degree  of  freedom  vehicle  model  (see  Figure  5)  is  considered  that  includes  lateral  acceleration,  yaw  and  roll 
dynamics.  The  roll  and  yaw  moments  of  inertia  are  represented  by  and  7^  respectively,  m  is  the  total  vehicle  mass,  ms 
is  the  sprung  mass  and  v  is  the  longitudinal  velocity  of  the  vehicle.  The  front  wheel  steering  angle  is  represented  by  5. 
The  linearized  equations  for  this  model  are  given  as: 


(21) 


mv{^+iy)-mshq>  =  YjF  =Cf{5- 


VlL-fr+C^-P) 

V  V 


/2^=ZM2=CW-^-/J)i-Cr(^- P)lr  (22) 

V  V 

(Ixx  +  msh2)q>=  Y.MX  -  msgh(p+  mshv(/3+  if/)  +MS  (23) 

where  Cf  and  Cr  are  the  cornering  stiffnesses  of  the  lumped  front  and  rear  wheels,  and  If  and  lr  are  respectively  the 
distances  of  the  front  and  rear  axles  from  the  eg.  The  suspension  moment,  represented  by  Ms,  is  given  as: 


Ms  =-(kf+kr)<p-(bf+br)<p 


(24) 


where  kfand  kr  are  the  stiffnesses  and  bf  and  br  are  the  damping  factors  for  the  front  and  rear  axles  respectively.  It  should 
be  noted  that  there  is  uncertainly  associated  with  the  estimation  of  the  values  of  the  above  vehicle  parameters.  This 
uncertainty  will  be  considered  in  the  present  analysis. 


Fig.  5.  Vehicle  Model  for  Mobility  Analysis 

For  measuring  vehicle  mobility,  a  simple  rollover  coefficient  is  employed  that  is  similar  to  the  one  described  in  [24]. 
Using  the  principle  of  balance  of  moments  and  vertical  forces,  a  rollover  metric  for  the  linear  model  above  is  given  by: 


R  = 


2  ni  ( 

- -(K  +  h)[  v(p+if/)-hcp 

mgyw  V 


(25) 


where  ha  is  the  height  of  the  roll  axis  above  the  ground  and  yw  is  the  track  width.  For  this  metric,  absolute  values  of  R 
that  are  greater  than  1  indicate  vehicle  wheel  liftoff  and  thus  impending  rollover. 

3.2.1  Application  of  SRSM 

We  define  the  state  space,  X  as: 


X  = 


P  V  <P  <P 


(26) 


The  variables  related  to  suspension  stiffness  are  represented  as  polynomial  chaos  expansions,  using  Heimite  polynomials 
of  standard  normal  random  variables.  Here,  ^  and  §2,  which  are  used  to  represent  the  input  uncertainty  in  the  system  The 
front  and  rear  axle  roll  stiffness  are  considered  to  be  normally  distributed  about  their  mean  values.  This  is  represented  as: 

kf  =  Mk;  +&kf  (2?) 

K  =  mk+4 2  <7*, 


(28) 


Then  the  state  variables  can  be  represented  as: 

^^)  =  Z^(01>y© 

j-o 

J--0 

where  §  =  [£,£]■ 


(29) 

(30) 

(31) 

(32) 


Hie  parameter  values  considered  for  the  steering  stillness  are  shown  in  Table  4. 
Table  4.  Uncertain  Vehicle  Parameters  in  Rollover  Analysis 


Parameter 

Mean 

Std.  Dev, 

(Nm/nid) 

(Nm/rad) 

kf 

30*103 

4^1  O’ 

k. 

30xl03 

4x1 0* 

A  spectral  stochastic  analysis  [11],  [20]  is  performed  using  the  above  expansions  to  obtain  the  time  evolution  of  the 
rollover  coefficient,  subject  to  various  steering  input  functions  (sinusoidal,  ramp-like  and  a  double  lane  change 
maneuver). 


4,  RESULTS 


4A  Traversal  over  Uncertain  Terrain 

Results  from  analysis  of  the  mobility  prediction  scenario  described  in  Section  3. 1  are  presented  using  SMC,  LHSMC  and 
SRSM  methods  are  presented  for  inclination  angles  (yf)  of  6°  and  1 5°  (see  Figure  6).  It  can  be  seen  that  the  distributions 
generated  by  the  SRSM  method  are  nearly  identical  to  those  generated  by  the  SMC  and  LHSMC  methods.  Increasing 
the  number  of  SMC  and  LHSMC  runs  slightly  decreases  distribution  variance,  however  in  both  cases  the  differences 
among  the  three  methods  is  small. 
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Fig.  6.  Probability  plots  for  mobility  prediction  scenario,  lor  small  number  of  SMC  and  LHSMC  runs  (lell)  and  large  number  (right) 

The  coefficient  values  obtained  ibr  a  2rid  order  expansion  of  drawbar  pull  (19)  considering  cf  q>  as  the  uncertain 
parameters  were: 


ao  =  -1.4260,  aj  =  0.298  L  a2=  0 .5586,  a3=  0.0000,  a4  =  0.009  L  a,  =  0.0000, 


The  results  predict  that  increasing  the  robot’s  initial  velocity  increases  the  probability  of  safe  slope  traversal,  as  expected. 
Also,  the  minimum  initial  velocity  required  for  successful  traversal  increases  as  the  inclination  increases.  A  clearly 
defined  “transition  region”  can  be  observed,  where  the  probability  of  safe  traversal  is  a  function  of  terrain  parameter 
variance  as  well.  This  region  effectively  describes  the  “risk”  of  traversal  at  a  certain  critical  velocity  range. 

Also,  the  computation  time  of  the  SRSM  method  is  compared  to  SMC  and  LHSMC  in  Table  5,  for  the  case  of  a  terrain 
inclination  angle  of  15  degrees.  It  can  be  seen  that  the  proposed  approach  results  in  a  significant  computational 
reduction  compared  to  the  baseline  approaches.  This  suggests  that  on-line,  real  time  implementation  of  the  method  is 
feasible  for  simple  models. 

Table  5.  Computation  Time  for  Mobility  Prediction  Analysis 


Method 

Simulation 

Runs 

Time 

Taken 

(sec) 

SMC 

5000 

55.750 

LHSMC 

1000 

11.343 

SRSM  (2nd  order) 

0.406 

4.2  Rollover  Analysis  considering  Vehicle  Parameter  Uncertainty 

Results  from  analysis  of  the  rollover  scenario  described  in  Section  3.2  are  presented  here.  Simulations  for  various 
vehicle  maneuvers  were  conducted  using  the  stochastic  response  surface  method  (SRSM),  standard  Monte  Carlo  (SMC) 
and  Monte  Carlo  Latin  Hyper  cube  Sampling  (LHSMC).  The  accuracy  of  the  SRSM  is  compared  to  that  of  the  SMC  and 
LHSMC  and  the  plots  are  presented  in  Figures  7-9.  Close  agreement  between  the  three  methods  can  be  observed. 


Fig.  7.  Vehicle  Rollover  Analysis  for  Sinusoidal  Steering  Input  using  Various  Statistical  Analysis  Techniques 
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Fig.  8.  Vehicle  Rollover  Analysis  for  Ramp-Like  Steering  Input  using  Various  Statistical  Analysis  Techniques 


Fig.  9.  Vehicle  Rollover  Analysis  for  Double  Lane  Change  Steering  Input  using  Various  Statistical  Analysis  Techniques 

Stochastic  analysis  allows  insight  into  the  range  of  the  variation  of  an  output  time  series.  In  Figure  10,  results  are  shown 
for  the  steering  angle  and  rollover  coefficient  for  a  double  lane  change  maneuver,  here  including  uncertainty  bounds  on 
the  2 a  variation.  In  this  particular  analysis,  it  can  be  observed  that  while  the  mean  absolute  value  of  the  rollover  metric 
(corresponding  closely  to  the  result  from  a  deterministic  simulation)  remains  less  than  one,  the  uncertain  value  exceeds 
one,  thus  indicating  a  substantial  risk  of  vehicle  rollover  when  parameter  uncertainty  is  explicitly  considered. 


Time(s) 


Fig.  10  Vehicle  Rollover  Analysis  Using  SRSM 

The  simulation  times  for  the  approaches  are  also  compared  (see  Table  6)  for  the  various  steering  inputs.  Computation 
time  for  the  SRSM  method  is  approximately  two  orders  of  magnitude  lower  than  for  LHSMC.  All  computations  were 
performed  on  a  desktop  PC  running  unoptimized  Matlab  code. 

Table  6.  Simulation  Times  using  Various  Statistical  Analysis  Techniques  for  Vehicle  Rollover  Analysis 


Steering  Input 

SMC 

(2000  RUNS) 

LHSMC 
(400  RUNS) 

SRSM 

Sinusoidal 

1920.1  s 

793.688  s 

6.691  s 

Ramp -like 

1933.5  s 

796.219  s 

6.766  s 

Double  Lane  Change 

1952.7  s 

808.235  s 

6.797  s 

4.  5.  CONCLUSION 


This  paper  has  presented  an  approach  to  statistical  mobile  robot  mobility  prediction  based  on  the  stochastic  response 
surface  method.  This  approach  explicitly  considers  uncertainty  present  in  vehicle  and  terrain  physical  parameter 
estimates.  Simulation  results  of  simplified  mobility  prediction  scenarios  have  shown  that  the  proposed  method 
represents  a  significant  improvement  over  conventional  Monte  Carlo  methods  in  terms  of  computational  efficiency,  and 


can  be  used  for  robustly  and  efficiently  predicting  the  traversability  of  mobile  robots  in  unstructured  environments. 
Current  work  is  focused  on  statistical  modeling  of  more  complex  three  dimensional  UGV  models. 
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On  the  Optimality  of  Dubms  Paths 
across  Heterogeneous  Terrain 

<3 

Ricardo  G.  Sanfelice  and  Emilio  Frazzoli 


Abstract 


We  derive  optimality  conditions  for  the  paths  of  a  Dubins  vehicle  when 
the  state  space  is  partitioned  into  two  patches  with  different  vehicle’s 
forward,  velocity  We  recast  this  problem  as  a  hybrid  optimal  control 
problem  and  solve  it  using  optimality  principles  for  hybrid  systems. 
Among  the  optimality  conditions,  we  derive  a  “refraction”  law  at  the 
boundary  of  the  patches  which  generalizes  the  so-called  Snell’s  law 
of  refraction  in  optics  to  the  case  of  paths  with  bounded  maximum 
curvature. 


1  Introduction 


Control  algorithms  that  are  capable  of  steering  autonomous  vehicles  to  satisfy 
a  given  set  of  specifications,  like  initial  and  final  constraints,  and  at  the 
same  time,  guarantee  certain  optimality  conditions  are  very  appealing  to 
applications  in  robotics  and  aerospace.  This  has  led  researchers  to  strive  for 
control  design  tools  that  adequately  incorporate  both  trajectory  constraints 
and  measures  of  optimality.  As  a  consequence,  many  results  from  the  theory 
of  optimal  control,  in  particular,  those  that  guarantee  time  optimality,  have 
found  wide  applicability  in  autonomous  vehicle  control  problems. 

Perhaps,  the  earliest  result  on  time-optimal  control  laws  for  autonomous 
vehicles  modeled  as  a  particle  moving  with  constant,  positive  forward  veloc¬ 
ity  and  with  constrained  minimum  turning  radius  is  the  work  by  Dubins  [6|. 
While  Dubins  used  only  geometric  arguments  to  establish  his  results,  a  few 
years  later,  the  appearance  of  Pontryagin’s  Maximum  Principle  in  [11  ]  en¬ 
abled  the  authors  in  [3|  to  systematically  recover  Dubins  results.  Moreover, 


building  from  the  work  of  Reeds  and  Shepp  [12],  the  application  of  Pontrya.- 
gin’s  optimality  principle  permitted  the  authors  in  [20,  3]  to  derive  similar 
results  for  a  vehicle  model  without  forward  velocity  constraints. 

In  this  paper,  we  consider  autonomous  vehicles  with  dynamics  governed 
by 


{x  =  v  sin  6 

y  =  vcosd  ,  (1) 

9  =  it 

where  [x,y)  is  the  vehicle’s  position,  9  is  the  angle  between  the  vehicle  and 
the  vertical  axis  determining  the  vehicle’s  orientation,  u  is  the  angular  ac¬ 
celeration  input  for  the  vehicle,  and  v  is  the  vehicle’s  forward  velocity.  This 
vehicle  model  is  usually  referred  to  as  Dubins  vehicle.  We  consider  the  case 
of  heterogeneous  velocity  along  the  terrain  where  the  vehicle  is  deployed. 
Two  different  velocities,  and  v2,  define  the  constant,  forward  velocity  of 
Dubins  vehicle  on  two  patches  of  the  plane,  patch  f\  and  patch  V2,  depicted 
in  Figure  1.  We  are  interested  in  the  following  problem: 

Find  the  minimum-time  path  for  Dubins  vehicle  from  an  ini¬ 
tial  point  and  angle  in  patch  V\  to  a  final  point  and  angle  in 


Figure  1  shows  possible  initial  and  final  vehicle  configurations,  which  are 
denoted  by  (x°,  t/°,0°)  arid  (x1,?/1,#1),  respectively,  for  which  a  minimum¬ 
time  path  is  to  be  found.  To  the  best  of  our  knowledge,  the  problem  described 
above  has  not  been  addressed  in  the  past,  perhaps  due  to  the  fact  that  the 
classical  Pontryagin’s  Maximum  Principle  is  not  applicable  because  of  the 
discontinuous  behavior  at  the  common  boundary  between  the  patches. 

By  recasting  this  problem  into  an  optimal  hybrid  control  problem  and  ap¬ 
plying  principles  of  optimality  for  hybrid  systems,  we  establish  the  following 
conditions  that  illuminate  important  characteristics  of  optimal  paths: 

•  The  portions  of  the  paths  that  remain  in  either  patch  are  Dubins  opti¬ 
mal. 

•  Optimal  paths  are  such  that,  at  the  boundary  between  the  patches ,  their 
type  does  not  change ;  that  is,  the  type  of  path  right  before  and  after 
crossing  the  boundary  are  the  same. 


•  Optimal  paths  that  cross  the  boundary  describing  a  straight  line  are 
orthogonal  to  the  boundary. 

•  The  angles  of  the  path  pieces  before  and  after  crossing  the  boundary 
satisfy  a  “refraction”  law,  which  consists  of  a  generalization  of  Snell’s 
law  of  refraction  in  optics. 

Applications  of  these  results  include  optimal  motion  planning  of  au¬ 
tonomous  vehicles  in  environments  with  obstacles,  different  terrains  prop¬ 
erties,  and  other  topological  constraints.  Strategies  that  steer  autonomous 
vehicles  across  heterogeneous  terrain  using  Snell’s  law  of  refraction  have  al¬ 
ready  been  recognized  in  the  literature  and  applied  to  point-mass  vehicles; 
see,  e.g.,  [1,  13].  Our  results  extend  those  to  the  case  of  autonomous  vehicles 
with  Dubins  dynamics. 

The  remainder  of  the  paper  is  organized  as  follows.  Section  2  discusses 
related  background  to  the  optimal  control  problem  outlined  above  and  intro¬ 
duces  general  notation.  In  Section  3,  we  present  a  hybrid  model  which,  as 
shown  in  that  same  section,  enable  us  to  formulate  the  problem  of  study  in 
an  optimal  hybrid  control  framework.  In  Section  4,  we  establish  necessary 
conditions  for  optimality  of  paths  including  a  refraction  law  at  the  boundary 
of  the  patches.  Due  to  space  constraints,  the  technical  proofs  are  omitted 
and  will  be  published  elsewhere. 


■y  =  o 


Figure  1:  Dubins  vehicle  on  an  heterogeneous  terrain.  The  initial  configu¬ 
ration  is  given  by  6°)  and  the  final  configuration  by  (a:1,?/1,#1).  The 

forward  velocity  in  patch  V\  is  smaller  than  the  forward  velocity  in  patch  P2- 
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2  Background 

Pontryagin’s  Maximum  Principle  [11]  is  a  very  powerful  tool  to  derive  neces¬ 
sary  conditions  for  optimality  of  solutions  to  a  dynamical  system.  In  words, 
this  principle  establishes  the  existence  of  an  adjoint  function  with  the  prop¬ 
erty  that,  along  optimal  system  solutions,  the  Hamiltonian  obtained  by  com¬ 
bining  the  system  dynamics  and  the  cost  function  associated  to  the  optimal 
control  problem  is  minimized.  In  its  original  form,  this  principle  is  applicable 
to  optimal  control  problems  with  dynamics  governed  by  differential  equations 
with  continuously  differentiable  right-hand  sides. 

The  shortest  path  problem  between  two  points  with  specific  tangent  di¬ 
rection  and  bounded  maximum  curvature  has  received  wide  attention  in  the 
literature.  In  his  pioneer  work  in  [6j,  by  means  of  geometric  arguments, 
Dubins  showed  that  optimal  paths  to  this  problem  consist  of  a  smooth  con¬ 
catenation  of  no  more  than  three  pieces,  each  of  them  describing  either  a 
straight  line,  denoted  by  £,  or  a  circle,  denoted  by  C  (when  the  circle  is 
traveled  clockwise,  we  write  C+,  while  when  the  circle  is  traveled  counter- 
clockwise,  we  write  C-),  and  are  either  of  type  CCC  or  C£C,  that  is,  they  are 
among  the  following  six  types  of  paths 

C~C+C~,  C+C-C+ ,  C~£C~,  C+CC+,  C+£C~,  C~£C+,  (2) 

in  addition  to  any  of  the  subpaths  obtained  when  some  of  the  pieces  (but 
not  all)  have  zero  length.  More  recently,  the  authors  in  [3]  recovered  Du- 
bins’  result  by  using  Pontryagin’s  Maximum  Principle;  see  also  [20|.  Further 
investigations  of  the  properties  of  optimal  paths  to  this  problem  and  other 
related  applications  of  Pontryagin’s  Maximum  Principle  include  1 1 6,  2,  4],  to 
just  list  a  few. 

Optimal  control  problems  exhibiting  discontinuous/ impulsive  behavior, 
like  the  heterogeneous  version  of  Dubins’  problem  outlined  in  Section  1,  can¬ 
not  be  solved  using  the  classical  Pontryagin’s  Maximum  Principle.  Exten¬ 
sions  of  this  principle  to  systems  with  discontinuous  right-hand  side  appeared 
in  [17]  while  extensions  to  hybrid  systems  include  (IS | ,  [7j,  and  jl5j.  These 
principles  establish  the  existence  of  an  adjoint  function  which,  in  addition  to 
conditions  that  parallel  the  necessary  optimality  conditions  in  the  principle 
by  Pontryagin,  satisfies  certain  conditions  at  times  of  discontinuous/jumping 
behavior.  The  applicability  of  these  principles  to  relevant  problems  have 
been  highlighted  in  ]  18,  10,  oj.  These  will  be  the  key  tool  in  deriving  the 
results  in  this  paper. 
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2.1  Notation 


We  use  the  following  notation  throughout  the  paper.  Rn  denotes  n-dimensional 
Euclidean  space.  R  denotes  the  real  numbers.  R>0  denotes  the  nonnegative 
real  numbers,  i.e. ,  R>0  =  [0,oo).  N  denotes  the  natural  numbers  including 
0,  i.e.,  N  =  {0, 1, . . .}.  Given  k  e  N,  N<fc  denotes  {0, 1, ...,  k}.  Given  a  set 
S,  S  denotes  its  closure  and  S°  denotes  its  interior.  Given  a  vector  x  G  Rn, 
|a;|  denotes  the  Euclidean  vector  norm.  Given  U  :=  [—1,1],  U  denotes  the 
set  of  all  piecewise-continuous  functions  u  from  subsets  of  R>o  to  U . 

3  Problem  Statement 

In  this  section,  we  formulate  the  problem  of  steering  Dubins  vehicle  across 
heterogeneous  terrain  as  a  hybrid  optimal  control  problem.  We  present  a 
hybrid  model  and  introduce  the  optimal  control  problem.  An  alternative 
approach  is  to  treat  this  problem  as  a  differential  equation  with  discontin¬ 
uous  right-hand  side  and  use  the  results  in  [17].  However,  a  hybrid  control 
systems  approach  is  not  only  more  convenient  from  a  modeling  point  of  view 
as  it  enables  the  use  of  a  sound  concept  of  solution  but  also  facilitates  the 
application  of  more  explicit  optimality  principles  for  hybrid  systems,  like  the 
ones  in  [18], 

3.1  Hybrid  model 

We  denote  by  7 iv  the  hybrid  system  that  captures  the  dynamics  of  Dubins 
vehicle  along  the  patches.  Let  £  R>o,  ^  'W  be  the  forward  velocity 
of  the  vehicle  on  patch  V\  and  patch  V2,  respectively,  where 

V1:={[xye]T  e  R3  \  y>0}  ,  V2  :=  {[x  y  0}T  e  R3  \y<0}  , 

which  share  a  common  boundary  Vi  {IV2  =  { [x  y  d] T  £  R3  |  y  =  0  } ;  see 
Figure  1.  Let  q  be  a  discrete  state  taking  value  in  Q  :=  {1,2}  that  indicates 
the  current  patch  to  which  the  vehicle  belongs  to.  Following  the  vehicle’s 
dynamics  in  (1), 


1 
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0  J 
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with 


X 

va  sin  6 

£  *— 
s  * 

V 

e 

(0 

and 

fq\00  ■  = 

Vq COS  Q 
U 

define  the  continuous  dynamics  (or  flows)  of  fCv,  where  £  is  the  continuous 
state  and  u  E  U  is  the  control  input.  Then,  during  flows,  £  captures  the 
vehicle  dynamics  on  the  q- th  patch  while  q  remains  constant.  We  model  the 

change  of  patch  so  that  it  occurs  when  y  is  zero  and  the  vehicle  is  moving 
away  from  the  current  patch.  Then,  defining  a  function  s  :  Q  — ►  {—1,1} 
where  s(l)  =  — 1  and  s(2)  =  1,  the  discrete  dynamics  (or  jumps)  of  'Hv  are 
given  by 


% 

Q 


Omn'Po  and  s(q)vqcosd 


(4) 


which  implies  that  at  jumps  £  does  not  change  while  q  is  toggled  between  1 
and  2.  Finally,  we  denote  by  £  :=  |£T  q\ 1  the  full  state  of  Tlv. 

Following  the  hybrid  systems  framework  outlined  in  |8j  and  further  es¬ 
tablished  in  [9,  14],  we  can  rewrite  'Hv  as 


H» 


C  ::::::  f(0 u) 

Cf  =  9(0 


CeC 

CeD 


by  defining 


f(Ou) : 

9(0  :  = 


fqO,u) 

n 


£ 


3 -q 


,C:=  U(C,x{«}), 

qtzQ 

,  D  :=  (J  (Dq  x  {q}), 


where  Cq  :=  Vq  and  Dq  :=  {£  G  R3  |  y  =  0, s(q)vg cos 6  >  0}  for  each  q  €  Q. 
Then,  Ti.v  is  determined  by  the  data  (/,  C,  g,  D) ,  where  /  is  the  flow  map.  C  is 
the  flow  set,  g  is  the  jump  map ,  and  D  is  the  jump  set.  As  in  [8] ,  solutions  to 
Ti.v  are  given  by  hybrid  arcs  on  hybrid  time  domains.  Hybrid  time  domains 
use  a  variable  t  to  indicate  flow  time  and  an  index  j  to  keep  track  of  the 
number  of  jumps,  and  hence,  parameterize  solutions  by  (t,j).  A  subset  E  of 
R>o  xN  is  a  hybrid  time  domain  if  it  is  the  union  of  infinitely  many  intervals  of 
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the  form  [tj,  tj. |_i]  x  {j},  where  0  =  to  <  h  <  h  <  •  •  • ,  or  of  finitely  many  such 
intervals,  with  the  last  one  possibly  of  the  form  [tj,  tj+ 1]  x  {j},  \tj,tj+i)  x  {j}, 
or  [tj,  oo)  x  {j}.  (Note  that  the  t  component  of  elements  (t,j)  G  E  does  not 
uniquely  define  the  index  j  since,  in  this  framework,  multiple  jumps  at  the 
same  t  are  possible.)  Then,  given  a  control  input  u  G  U,  solutions  to  Ttv 
are  given  by  functions,  called  hybrid  arcs,  £  :  dom£  — >■  R4,  where  domC  is  a 
hybrid  time  domain,  t  i— >■  £(t,  j )  is  a  locally  absolutely  continuous  function  for 
each  fixed  j,  t  i— >•  q(t,j )  is  a  piecewise  constant  function  for  each  fixed  j,  and 
C  satisfies  the  flow  and  jump  conditions  mentioned  above.  More  precisely, 
given  an  input  u  G  U,  a  hybrid  arc  £  is  a  solution  to  the  hybrid  system  7 iv  if 
C(0,  0)  G  C  U  D,  domC  =  dom-u,  and: 

(51)  For  all  j  G  N  and  almost  all  t  such  that  (t,j)  G  domC  1, 

e  C,  c (t,j)  =  f(C(t,j),u(t,j ))  . 

(52)  For  all  (t,j)  G  dom^  such  that  (t,j  +  1)  G  dom^, 

C(t,j)eD,  C(t,j  + 1)  =  g(C(t,j))  ■ 

Inputs  u  given  as  signals  t  >  u(t)  for  each  t  G  R>o  can  be  rewritten  on  a 
hybrid  time  domain  E  by  defining,  with  some  abuse  of  notation,  u (t,j)  := 
uit )  for  each  (t,j)  G  E.  Note  that  solutions  to  7 ~LV  exist  from  every  point  in 
CU  D  =  R3  x  Q.  In  particular,  solutions  are  allowed  to  flow  in  the  boundary 
V\  n  V2  with  either  q  =  1  or  q  =  2;  such  a  feature  cannot  be  captured  with 
a  differential  equation  with  discontinuous  right-hand  side  or  with  a  (regular) 
differential  inclusion  without  adding  extra  solutions.  Also  note  that  since  the 
sets  Dq  are  not  closed  subsets  of  R3,  the  regularity  property  for  D  required 
in  [9,  14]  does  not  hold  (the  flow  map,  jump  map,  and  jump  set  of  7 iv  satisfy 
the  properties  therein).  While  such  a  regularity  is  not  required  for  the  results 
in  this  paper  to  be  true,  it  turns  out  that,  as  shown  in  [14],  it  highlights  the 
presence  of  undesirable  solutions  if  the  sets  Dq  were  to  be  closed  or  small 
noise  entered  through  the  state. 

3.2  Hybrid  optimal  control  problem 

We  consider  the  following  hybrid  optimal  control  problem.  Given  9 °)  G 

Cl  and  (a;1,?/1,#1)  G  C 

1C{t,j)  denotes  the  derivative  of  t  1 — s-  £ (t,j)  with  respect  to  t  for  a  fixed  j,  which  exists 
for  almost  every  t  such  that  (t,j)  G  dom  4  n  ([tj,  b+i]  X  {j}). 
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(*)  Minimize  the  transfer  time  T  G  M>o  subject  to: 

(Cl)  Dynamical  constraint:  dynamics  of  7 iv  given  in  (3)-(4). 

(C2)  Input  constraint:  u  G  W. 

(C3)  Initial  and  terminal  constraints:  every  optimal  solution  (£,<?)  to 
74,  satisfies  the  initial  constraint  (x(0, 0),y(0,  0),  #(0, 0))  =  ( x°,y°,9 °) 
and  the  terminal  constraint  (x(T,J),y(T,J),9(T,J))  =  (re1,?/1,#1) 
for  some  (T,  J )  G  dom(£,g). 

The  number  of  jumps  required  to  solve  (*)  is  finite,  given  by  J  —  1,  and  no 
smaller  than  one;  hence,  optimal  solutions  to  (*)  are  not  Zeno.  The  initial 
and  final  constraints  are  such  that  solutions  can  flow  from  some  time  before 
their  first  jump  and  after  their  final  jump  (that  is,  the  first  jump  is  at  some 
(fi,0)  with  t\  >  0  and  the  last  jump  is  at  some  (tj,  J  —  1)  with  tj  <  T). 
This  is  a  technical  requirement  for  the  application  of  the  hybrid  maximum 
principle  in  [18]  in  the  next  section. 

4  Necessary  conditions  for  optimality 

Necessary  optimality  conditions  for  solutions  to  Ftv  solving  (*)  can  be  ob¬ 
tained  using  the  principle  of  optimality  for  hybrid  systems  in  [18]  (see  also 
[19]  and  [10]).  Under  further  technical  assumptions,  Theorem  1  in  [18]  estab¬ 
lishes  that  there  exists  an  adjoint  pair  (A,  A0),  where  A  is  a  function  and  A0  is 
a  constant,  which,  along  optimal  solutions  to  (*),  satisfies  certain  Hamilto¬ 
nian  maximization ,  nontriviality ,  transversality ,  and  Hamiltonian  value  con¬ 
ditions.  In  particular,  [18,  Theorem  1]  can  be  applied  to  the  optimal  control 
problem  (*)  to  deduce  the  following  optimality  conditions  for  the  paths. 

Proposition  4.1  [properties  of  (*)]  For  each  optimal  solution  (f ,  q)  to  (*) 
with  optimal  control  u,  minimum  transfer  time  T ,  and  J—  1  number  of  jumps, 
there  exists  a  function  A  :  domA  — >■  M3,  A  :=  [a  [3  7]T,  domA  =  dom(£,g), 
where  t  >  A (t,j)  is  absolutely  continuous  for  each  j ,  ( t,j )  G  domA,  and  a 
constant  A0  Gl  defining  the  adjoint  pair  (A,  A0)  satisfying: 

a)  A0  >  0  and  A (t,j)  =  - — J^(£(f,  j),  X(t,j),  X0,u(t,  j))  for  almost 
every  t  G  [tj,tj+i],  ( t,j )  G  domA,  where,  for  each  q  G  {1,2},  Hq  : 
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R3xR3xRx  xU  — >■  R  is  the  Hamiltonian  associated  with  the  continuous 
dynamics  ofTCv,  which  is  given  by 

Hq(C  A,  A0,  u)  =  avq sin  8  +  f3vq  cos  9  +  ju  —  A0 


for  each  q  G  Q. 

b)  There  exist  a,f3  £  R  and ,  for  each  j  G  N<j7  there  exists  pj  G  R  such 

that  a(t,j )  :=  a  for  all  (t,j)  G  domg,g)7  :=  f3  +  pj  for  almost 

all  t  G  [0,T],  (t,j)  G  domg,g)7  and'y(t,j)  =  7 (t,j  +  1)  for  each  ( t,j ) 
such  that  +  1)  G  domA. 

c)  For  every  ( t,j )  G  dom (£,q)  such  ihat'y(t,j)  0,  u(t,j)  =  sgn£(t,  j)) ; 
and  for  every  (t,j)  G  dom(£,g)  such  that 'jit,  j)  =  0,  u(t,j )  =  0. 

d)  For  every  (t,j)  G  doing,  g)  such  that'yft^j)  =  07  /3(t,j)  tan 9(t,j)  = 
cyftj). 


Remark  4.2  The  proof  of  Proposition  Jhl  uses  the  fact  that  TLV  can  be  as¬ 
sociated  with  a  hybrid  system  TPV  given  in  the  framework  in  [18]  and  that 
every  solution  to  Ttv  solving  (*)  is  also  a  solution  to  U*  (agreeing  with  the 
concept  of  solution  in  [18]  2).  This  property  follows  by  construction  of7i%. 
Hybrid  systems  in  [18]  and  [10]  have  a  continuous  state  £  with  flows  governed 
by  £  =  fq(f,u)  when  £  belongs  to  a  smooth  manifold  A4q;  where  q  £  Q  is 
a  discrete  state  (which  remains  constant  during  flows).  Jumps  from  mode  q 
to  mode  ef  satisfy:  1)  the  switching  condition  (£,£')  G  Sq/4t 7  where  £  is  the 
continuous  state  before  the  jump ,  £'  is  the  continuous  state  after  the  jump , 
and  Sq^qf  is  the  switching  set;  and  2)  a  temporal  constraint  enforcing  that  the 

2 In  [18],  solutions  to  hybrid  systems  are  given  on  compact  time  intervals  by  abso¬ 
lutely  continuous  functions  fy-  on  [fy,fy+ 1]  such  that,  for  each  j  £  {l,2,...,z/}  (with 
finite  v  £  N)  and  for  finite  sequences  of  logic  states  and  control  inputs  sat¬ 
isfy  the  flow  condition  fy'  =  fq.  (fj (^),  for  almost  all  t  £  [fy,fy+ 1]  and  the  jump 

condition  (fy(fy),  fy+i(fy))  £  Sq.tq.+1  for  each  fy,  where  ij  denotes  the  jump  time  (which 
is  assumed  to  belong  to  the  interior  of  the  compact  time  interval  where  solutions  are 
defined)  and  Sq.tq.+1  is  the  switching  set  at  the  j- th  jump  (see  [18,  Definition  3]  for 
more  details).  Hence,  passing  from  a  solution  (  ona  bounded  hybrid  time  domain  donfy 
with  jumps  at  different  (fyfy)’s,  first  jump  at  (fy,0)  with  t\  >  and  last  jump  at 
(fy,  J  —  1)  with  tj  <  T,  where  T  :=  sup  {t  £  M>o  |  £  N  such  that(^,fy  £  donfy  }  and 

J  \=  sup  {j  £  N  |  3t  £  M>o  such  that(^,  j)  £  dom£  }),  to  a  solution  as  in  [18,  Definition  3] 
is  straightforward. 
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jump  time  for  the  current  mode  is  in  the  set  Jq  C  E.  To  obtain  TL*,  the  sets 
Cq  in  TLV  are  replaced  by  smooth  manifolds  M.q,  Cq  C  M.q,  while  the  jump 
set  and  the  jump  map  are  replaced  by  the  switching  condition  given  by 


Si, 2  =  S 


2,1 


=  S  ■■=  {(£,£) 


=  o,£ 


R' 


and  Ji  =  J2  =  K.  Then,  the  properties  of  the  adjoint  pair  guaranteed  by 
1 18,  Theorem  lj  automatically  imply  item  a)  in  Proposition  j.t  (see  [18, 
Definition  9[).  The  condition  for  optimality  at  switches  for  the  adjoint  state 
A  implies  that  only  the  second  component  of  X,  i.e.  0,  has  a  jump  while  the 
other  two  components  are  continuous  (see  Remark  4-3)-  This  implies  item 
b)  in  Proposition  4-1.  The  Hamiltonian  maximization  condition  guaranteed 
to  hold  by  [18,  Theorem  lj  implies  that 


Hgit.j) (fi (t,  j) ,  X(t,  j) ,  X 0,u(t,  j)) 


max  Hg{tS)(( (t,  j) ,  A (t,  j) ,  A:) ,  w) 

WEU  '  ' 


for  almost  every  t  €  \tj,tj+1\,  (t,j)  €  dom A  (see  [18,  Definition  lOj).  It 
follows  that  the  control  law  in  item  c)  in  Proposition  f.l  maximizes  Hq. 
By  integrating  the  adjoint  state  A  when  u  =  0,  Proposition  4 -l.d  follows 
automatically.  ■ 


Remark  4.3  [18,  Theorem  lj  implies  that  at  jumps,  the  optimal  solution, 
optimal  control,  and  adjoint  pair  satisfy  the  switching  condition  (—A [  t,  j ) ,  A(t,  j+ 
1))  €  Kj  for  each  j  for  which  there  exists  t  £  [0,T]  such  that  (t,j),(t,j  + 

1)  €  dom  A,  where  Kj-  is  the  polar  of  the  Boltyanskii  approximating  cone  to 
Sq(tj),q(tj+i)  (=  S).  The  set  S  is  such  that  Kj-  is  given  by 


{w,  V) 


0  Vv  £  S  | 


since  the  Boltyanskii  approximating  cone  to  S  is  the  set  itself.  Then,  since 
by  definition  of  S  the  second  and  fourth  components  of  v  in  Kj~  are  zero, 
(~X(t,j),  X(t,j  +  1))  £  Kj~  if  and  only  if  aft,  j)  =  a(t,j  +  1),  j(t,j)  = 
jlt,j  +  1),  which  implies  that  only  [3  can  have  a  jump.  This  property  can 
also  be  obtained  using  the  optimality  principles  in  [15].  ■ 
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4.1  Optimality  of  paths 

The  properties  of  the  adjoint  pair  (A,  A0)  and  the  control  input  u  in  Propo¬ 
sition  4.1  can  be  related  to  properties  of  the  continuous  component  £  of  the 
solutions  to  (-A-).  These  characterize  the  optimal  paths  from  given  initial  and 
terminal  constraints,  as  the  following  theorem  states. 

Theorem  4.4  [optimality  conditions  of  solutions  to  (k)j  Each  optimal  solu¬ 
tion  (fi,q)  to  (*)  with  optimal  control  u,  minimum  transfer  time  T,  and  J—  1 
num  ber  of  jumps  is  such  that: 


a)  The  continuous  component  f  is  a  smooth  concatenation  of  finitely  many 
pieces  from  the  set  {C+,C  ,£}. 

b )  The  input  component  it  is  piecewise  constant  with  finitely  many  pieces 
taking  value  in  — 1,0,1}. 

c)  Each  piece  of  the  continuous  component  £  contained  in  Cqi  q  G  Q,  is 
Dubins  optimal  between  the  first  and  last  point  of  such  piece,  i.e.,  it  is 
given  as  in  (2). 

d)  For  each  ( t,j )  G  dom(£,g)  for  which  (x(t,j),y(t,j),9(t,j))  e  Dq(tj), 
the  solution  has  a  jump  and: 

d.  l)  If  the  path  before  the  jump  is  C  then  the  path  after  the  jump  is  C. 

d.2)  If  the  path  before  the  jump  is  L  then  the  path  after  the  jump  is  L 
and  6(t,j)  is  zero  or  any  multiple  of  n. 

Remark  4.5  The  proof  of  Theorem  4-4  uses  Proposition  4-1  and  the  fact 
that,  since  the  jump  condition  in  hiv  is  time  independent  (that  is,  J\  ------  J2  ------ 

E ),  the  Hamiltonian  value  condition  guaranteed  to  hold  by  [18,  Theorem  1[ 
implies  that  there  exists  h*  G  K  such  that 

hr  =  j) ,  j)) 

for  almost  every  t  G  ( t,j )  G  domA  (see  [18,  Definition  13[).  ■ 
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Figure  2  depicts  optimal  paths  around  the  boundary  of  the  patches.  Item 
d.l)  in  Theorem  4.4  implies  that  optimal  paths  that  cross  the  boundary  are  of 
the  same  type  at  each  side  of  it.  More  precisely,  if  before  crossing  the  bound¬ 
ary,  the  optimal  path  is  of  type  C  ( C+  or  C~ ),  then  the  optimal  path  after 
crossing  the  boundary  is  also  of  type  C  ( C+  or  C~,  respectively).  Figure  2(a) 
depicts  an  optimal  path  of  type  C+.  Statement  d.2)  in  Theorem  4.4  implies 
that  £-type  paths  at  the  boundary  are  optimal  only  if  they  are  orthogonal 
to  the  boundary.  Figure  2(b)  depicts  this  situation. 


(a)  C+-type  of  path  at  the  bound¬ 
ary.  Path  pieces  C+  in  patch  Vi 
with  radius  ri  =  v\  and  in  patch 
V2  with  radius  =  v2,  V2>  v\. 


V 1 

- 3 - "Fi  rrp2 

r2 

(b)  £-type  of  path  at  the  boundary. 

The  angle  between  the  path  and  the 
boundary  in  each  patch  is  7r/2. 


Figure  2:  Optimal  paths  nearby  the  boundary:  paths  of  types  C+  and  £ 
satisfying  the  necessary  conditions  in  Theorem  4.4. 


Using  Theorem  4.4,  it  is  possible  to  determine  optimal  families  of  paths 
for  a  class  of  solutions  to  (*).  The  following  statements  follow  directly  from 
Dubins’  result  and  Theorem  4.4. 

Corollary  4.6  [optimal  paths  w/one  jump]  Every  optimal  solution  (£,<?)  to 
(*)  with  only  one  jump  is  such  that  the  continuous  component  £  is  a  smooth 
concatenation  ofC,C  paths  pieces  and  is  given  by  one  of  the  following  four 
types  of  paths 


Ci£iC2£2C3,  C1C2C3C4C5,  C1£1C2C3C4 


(5) 
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in  addition  to  any  such  path  obtained  when  some  of  the  path  pieces  ( but  not 
all)  have  zero  length.  Furthermore,  if  the  path  piece  intersecting  the  boundary 
is  of  type  C,  then  the  continuous  component  £  describes  a  path  of  type  C1£1C2 
(or  any  such  path  obtained  when  C\  and/or  C2  have  zero  length). 

A  consequence  of  Theorem  4.4  that  is  useful  when  computing  optimal 
paths  is  the  following. 

Corollary  4.7  [nonoptimal  paths]  For  the  optimal  control  problem  (*),  solu¬ 
tions  toHv  satisfying  (C1)-(C3)  with  the  continuous  component  £  describing 
paths  that  change  at  the  boundary  are  nonoptimal,  that  is,  paths  that  before 
and  after  the  boundary  are  given  by  C+  and  C,  C~  and  C,  C  and  C+ ,  C,  and 
C~ ,  C+  and  C~ ,  or  C~  andC+,  respectively,  are  nonoptimal. 

Figure  3  depicts  two  of  the  path  types  that  Corollary  4.7  determines  to 
be  nonoptimal. 


(a)  Nonoptimal  C+/C~-type  path  at 
the  boundary.  Path  piece  C+  in 
patch  Vi  with  radius  ri  =  v\  and 


(b)  Nonoptimal  £/C“-type  path  at 
the  boundary.  Path  piece  C~  in 
patch  V2  with  radius  r2  =  v^. 


path  piece  C  in  patch  V2  with  ra¬ 
dius  r2  =  V 2,V2>  Vi. 

Figure  3:  Nonoptimal  paths  at  the  boundary:  paths  of  type  C+/C-  and  £/C_ 
changing  at  the  boundary  and  hence,  not  satisfying  the  necessary  conditions 
for  optimality  in  Theorem  4.4. 
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4.2  Refraction  law  at  boundary 

The  optimal  control  law  given  in  Proposition  4.1.c  and  the  properties  of  the 
component  7  of  the  adjoint  state  A  given  in  Proposition  4.1.b  imply  that  the 
control  law  is  constant  at  jumps  of  TLV  (note  that  u  is  piecewise  continuous 
for  each  fixed  j  with  discontinuities  at  (f,  j)’s  where  the  path  type  changes). 
While  9  remains  constant  at  the  boundary,  the  initial  and  final  angles  (and 
their  variations)  of  the  paths  intersecting  the  boundary  satisfy  the  following 
algebraic  condition  involving  the  patch  velocities  v\  and  v2. 

Theorem  4.8  [refraction  law  for  (*)]  Let  (f,q)  be  an  optimal  solution  to  (*). 
Let  9i  and  62  denote  the  initial  and  final  angle,  respectively,  of  a  path  piece 
intersecting  the  boundary  Vi  CV2,  as  show  in  Figure  f.  Let  A9i,A92  €  R  be 
given  by  A9i  :=  9*  —9i,  A92  :=  #2  —  9*,  where  9*  is  the  angle  between  the  path 
and  the  boundary  V\  fl  V2  at  their  intersection  (with  respect  to  the  vertical 
axis).  If  the  path  piece  intersecting  V\  rrP2  is  of  type  C,  then  Vi,  v2,  #1,  92,  A<?i 
and  A92  satisfy 


vx  _  1  +  cot  92  cot  (AhzMi  +  A±A) 

v~2  ~  1  +  cot 0! cot  +  AiA)  ’  ^ 

and  if  the  path  piece  intersecting  Vi  fl  V2  is  of  type  C,  then  9i  and  92  are 
equal  to  n. 


Remark  4.9  Equation  (6)  in  Theorem  f.8  implies  that  for  a  path  of  type  C 
intersecting  V1FV2  to  be  optimal,  9\,92,  A9\  and  A92  shown  in  Figure  4  must 
satisfy  (6).  When  the  path  intersecting  V\C\V2  is  of  type  L,  by  Corollary  4-6, 
the  path  L  is  orthogonal  to  V\L\V2  and  consequently,  there  is  no  “refraction” 
at  the  boundary.  This  is  depicted  in  Figure  2(b).  The  proof  of  Theorem  4-8 
follows  from  the  properties  of  the  optimal  solution  and  adjoint  state  at  jumps 
stated  in  Theorem  4-4  and  Proposition  4-1. d.  ■ 

Equation  (6)  can  be  interpreted  as  a  refraction  law  at  the  boundary  of 
the  two  patches  for  the  angles  (and  their  variations)  9i,92  (and  A<?i,  A92). 
This  parallels  Snell’s  law  of  refraction  in  optics,  which  states  a  relationship 
between  the  angles  of  rays  of  light  when  passing  through  the  boundary  of  two 
isotropic  media  with  different  refraction  coefficients.  More  precisely,  given 
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V 1 


(a)  Refraction  for  CCC- type  of  path 
nearby  the  boundary.  The  C  path 
pieces  define  the  angles  and 

their  variations  A#i,  A#2* 


(b)  Refraction  for  CCC-type  of  path 
nearby  the  boundary.  The  tangents 
(plotted  with  lines)  at  the  point 
of  path  change  define  the  angles 
#i,  02  and  their  variations  A#i,  A62. 


Figure  4:  Refraction  law  for  paths  at  the  boundary.  The  initial  and  final 
angles  of  optimal  paths  intersecting  the  boundary  given  by  01  and  d2 ,  re¬ 
spectively,  and  their  variations  (A0i,  A02)  satisfy  equation  (6),  which  is  a 
generalization  of  Snell’s  law  of  refraction. 


two  media  with  different  refraction  indexes  v1  and  v2 ,  Snell’s  law  of  refraction 
states  that 

Vi  =  sinfli 

v2  sin  92  ’ 

where  9\  is  the  angle  of  incidence  and  92  is  the  angle  of  refraction.  This 
law  can  be  derived  by  solving  a  minimum-time  problem  between  two  points, 
one  in  each  medium.  Moreover,  the  dynamics  of  the  rays  of  light  can  be 
associated  to  the  differential  equations  x  =  v^,  where  Vi  is  the  velocity  in  the 
*-th  medium,  i  =  1,2.  Theorem  4.8  generalizes  Snell’s  law  to  the  case  when 
the  dynamics  of  the  rays  of  light  are  given  by  (1).  In  fact,  (6)  reduces  to 
(7)  when  A#i  =  9i  and  A 02  =  02.  In  the  context  of  autonomous  vehicles, 
(6)  consists  of  a  generalization  of  the  refraction  law  for  optimal  steering  of  a 
point-mass  vehicle,  as  in  [1,  13],  to  the  Dubins  vehicle  case. 
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To  further  illustrate  our  results,  consider  =  2v2  >  0,  (x°,y°,d0),  and 
91)  as  depicted  in  Figure  5.  A  path  corresponding  to  a  solution  to  7iv 


Vi 


{x\yl,el) 


s' 


r2 


— 


(a)  (Nonoptimal)  path  of  type  CLC. 


(b)  Path  of  type  CLCLC. 


Figure  5:  Optimal  control  of  Dubins  vehicle  on  patches  with  velocities  Vi  = 
2v2.  The  path  depicted  in  (a)  is  nonoptimal  since  its  £-type  piece  is  not 
orthogonal  to  the  boundary  V\  0  V2  (it  is  also  nonoptimal  since  it  does  not 
exploit  the  fact  that  the  maximum  velocity  in  patch  Vi  is  twice  faster  than 
in  patch  V2).  The  path  depicted  in  (b)  is  a  candidate  for  optimality  as  it 
satisfies  the  conditions  in  Theorem  4.4  and  Corollary  4.6. 


matching  the  initial  and  terminal  constraints  is  shown  in  Figure  5(a).  Since 
the  £-type  path  piece  smoothly  connecting  the  C-type  paths  at  (x°,y°,0°) 
and  (ir1,?/1,#1)  does  not  intersect  the  boundary  V\  P i  V2  orthogonally,  The¬ 
orem  4.4.d  implies  that  it  is  nonoptimal  (see  also  Corollary  4.6).  Note  that 
this  path  is  not  taking  advantage  of  the  fact  that  in  patch  V\,  the  vehicle 
can  travel  twice  faster  than  in  patch  V2.  Paths  candidate  for  being  opti¬ 
mal  are  like  the  one  depicted  in  Figure  5(b)  as  it  satisfies  the  conditions  in 
Theorem  4.4  and  Corollary  4.6. 


5  Conclusions 

We  have  derived  necessary  conditions  for  the  optimality  of  paths  with  bounded 
maximum  curvature.  To  establish  our  results,  we  formulated  the  problem  as 
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a  hybrid  optimal  control  problem  and  used  optimality  principles  from  the 
literature.  Our  results  provide  verifiable  conditions  for  optimality  of  paths. 
These  include  conditions  both  in  the  interior  of  the  patches  and  at  their  com¬ 
mon  boundary,  as  well  as  a  refraction  law  for  the  angles  which  generalizes 
Snell’s  law  of  refraction  in  optics  to  the  current  setting.  Applications  of  our 
results  include  optimal  motion  planning  tasks  for  autonomous  vehicles  with 
Dubins  vehicle  dynamics. 
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A  Hybrid  Control  Framework  for 

Robust  Maneuver-based 
Motion  Planning 

Ricardo  G.  Sanfelice  and  Emilio  Frazzoli 


Abstract 


We  introduce  a  modeling  framework  for  robustness  of  maneuver- 
based  motion  planning  algorithms  for  nonlinear  systems  with,  sym¬ 
metries.  Our  framework  implements  a  hybrid  controller  that  robustly 
combines  motion  primitives,  which  consist  of  trim,  trajectories  and  ma¬ 
neuvers,  from  a  pre- defined  library.  The  closed- loop  system  is  viewed 
as  a  hybrid  system  with  flows  given  by  a  differential  equation,  jumps 
given  by  a  difference  equation,  and  two  sets  where  these  dynamics  are 
allowed.  We  show  that  our  hybrid  controller  for  implementation  of 
motion  planning  algorithms  confers  to  the  closed- loop  system  robust¬ 
ness  properties  to  a  large  class  of  perturbations. 


1  Introduction 

Motion  planning  algorithms  are  commonly  applied  in  robotics  as  a  method 
to  solve  steering  problems.  In  a  real-world  scenario,  the  motion  planning 
task  needs  to  be  accomplished  in  the  presence  of  obstacles,  measurement 
error,  exogenous  disturbances,  and  unmodeled  dynamics.  To  guarantee  some 
degree  of  robustness,  motion  planning  algorithms  are  usually  blended  with 
feedback  control  algorithms,  which  track  the  output  of  the  motion  planner; 
see,  e.g.,  [1,11,12,14,18], 

The  motion  planning  problem  itself  is  typically  recast  as  an  optimal  con¬ 
trol  problem  with  cost  function  and  constraints  stemming  from  the  given  task 
to  be  accomplished  along  with  its  specifications.  In  complex  motion  planning 
problems,  online  computation  of  optimal  control  policies  is  not  always  feasi¬ 
ble.  A  motion  planning  technique  suitable  in  such  cases  was  proposed  in  [6! 


for  general  nonlinear  systems  with  symmetries.  A  motion  plan  in  [6]  is  given 
by  a  concatenation  of  a  finite  number  of  motion  primitives  selected  from 
a  pre-defined  library  and  implemented  in  a  maneuver  automaton.  Motion 
primitives  were  defined  in  [6|  as  equivalence  classes  of  trajectories,  induced 
by  symmetries  in  the  system’s  dynamics,  e.g.,  invariance  with  respect  to  time, 
translations,  and  rotations. 

One  of  the  main  features  of  the  maneuver- mot  ion  based  approach  is  that 
each  element  in  the  motion  primitives  library  can  be  designed  off-line  subject 
to  particular  specifications,  like  optimality,  state  constraints,  etc.,  relaxing 
in  this  wav  on-line  computation  requirements;  see,  e.g.,  its  applications  to 
robotics  in  |5,  7,  IS] .  However,  this  method  combines  motion  primitives  in 
an  open-loop  manner,  which  restricts  its  application  to  nominal  scenarios, 
that  is,  those  without  perturbations.  Moreover,  the  fact  that  the  trajectories 
resulting  from  this  approach  are  not  necessarily  smooth,  renders  the  task 
of  robust ifying  motion  plans  via  feedback  control  challenging  since  standard 
trajectory  tracking  control  design  techniques  are  not  applicable. 

In  this  paper,  we  propose  a  hybrid  control  algorithm  that  executes  maneuver- 
based  motion  plans  and  combines  state  feedback  control  laws  for  nonlinear 
systems  with  symmetries.  The  purpose  of  our  hybrid  controller  is  to  provide 
a  control  framework  for  maneuver-based  motion  planning  featuring  robust¬ 
ness  properties  to  perturbations.  We  show  that  this  framework  results  in  a 
hybrid  system  with  implementable  semantics,  and  hence,  useful  experimental 
setups.  This  class  of  hybrid  systems  has  been  recently  introduced  in  [8,  9] 
motivated  by  the  pursue  of  robustness  of  asymptotic  stability.  Our  control 
framework  for  maneuver-based  motion  planning  also  borrows  ideas  from  the 
techniques  in  jl.6|  for  robust  combination  of  state  feedback  and  open-loop 
controllers,  and  also  from  the  invariant  constructions  in  [4], 

The  paper  is  organized  as  follows.  Section  2  introduces  notation  and  basic 
definitions  regarding  nonlinear  systems  with  symmetries,  motion  primitives 
and  plans,  and  hybrid  systems.  Section  3  introduces  our  hybrid  control 
framework  for  motion  planning,  while  Section  4  states  its  main  properties. 


2  Preliminaries 

2.1  Notation 

R  denotes  the  real  numbers.  R>o  denotes  the  nonnegative  real  numbers, 
i.e.,  R>o  =  [0,  oo).  N  denotes  the  natural  numbers  including  0,  i.e.,  N  = 
{0,1,.. .}.  N<fe  (N<fc)  denotes  numbers  in  N  from  0  to  k  —  1  (from  0  to 
k,  respectively).  Rn  denotes  the  n-dimensional  Euclidean  space.  ®  de¬ 
notes  the  open  unit  ball  in  a  Euclidean  space.  Given  a  set  S,  S  denotes 
its  closure  and  S°  denotes  its  interior.  Given  sets  Si, £2  subsets  of  Rn, 
£}  +  £2  :=  {aq  +  x2  |  x-i  E  Sllx2  E  S'2}.  Given  a  vector  x  E  Rn,  \x\  de¬ 
notes  its  Euclidean  norm.  The  equivalent  notation  [a;T  yT]T,  [2:  y]T,  and 
(x,y)  is  used  for  vectors.  Given  a  function  /  :  Rm  — >  Rn,  its  domain  of 
definition  is  denoted  by  dom /;  i.e.,  dom /  :=  {2  E  Rm  |  f(x)  is  defined}.  A 
function  a  :  R>0  — >•  R>o  is  said  to  belong  to  class  if  it  is  continuous,  zero 
at  zero,  strictly  increasing,  and  unbounded.  T’C°(R>0,Rm)  is  the  set  of  all 
piecewise  continuous  signals  j3  :  dom/3  — >•  Rm,  dom/3  C  R>o- 

2.2  Motion  planning  for  nonlinear  systems  with  sym¬ 
metries 

We  consider  nonlinear  control  systems  of  the  form 

V  :  x  =  f(x,u )  (1) 

where  /  :  Rn  x  Rm  — >■  Rn  is  a  locally  Lipschitz  function,  x  £  Rn  is  the 
state,  and  u  G  Rm  is  the  control  input.  We  focus  on  a  particular  subclass  of 
nonlinear  systems  V,  those  satisfying  certain  symmetry  properties.  Next,  we 
review  and  adapt  some  of  the  concepts  in  [6]  for  the  purposes  of  this  paper. 

2.2.1  Nonlinear  systems  with  symmetries 

A  large  class  of  mechanical  systems  are  invariant  under  certain  transforma¬ 
tions  of  their  state.  These  include  mobile  robots  as  well  as  more  general 
autonomous  vehicles,  like  several  helicopters  and  airplanes  models,  among 
others.  General  invariant  transformations  can  be  characterized  with  the  the¬ 
ory  of  Lie  groups  (see  [2]  for  an  introduction  to  Lie  groups  and  [13]  for 
applications  to  mechanics). 
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Let  Q  be  a  finite-dimensional  Lie  group,  and  let  e  be  its  identity  element. 
It  is  said  that  Tis  a  left  action  of  the  group  Q  on  Rn  if  T  :  Q  x  Rn  — >■  Rn 
is  a  smooth  map  such  that  \P(e,:r)  =  x  for  all  x  E  Rn  and  T(g,  T(/i,  a;))  = 
ty(gh,x)  for  all  g,h  E  Q ,  x  E  Rn.  Let  g  be  the  Lie  algebra  of  Q. 

Definition  2.1  (symmetry  ofV)  The  nonlinear  system  V  is  invariant  with 
respect  to  the  left  group  action  'S'  if  for  all  g  E  Q ,  a:0  E  Rn,  and  ji  E 
■pC0(R>o,  Rm),  each  solution  (in  the  appropriate  sense1)  to  V  starting  from 
x°  withuif)  =  denoted  hyt^f  f(x° ,  fi]t) ;  is  such  that  d/ (g ,  f{x° ,  t))  = 

x°),  ji]  t)  for  all  t  E  dom  <f.  ■ 

Definition  2.1  states  that  V  is  invariant  if  the  left  action  \I/  commutes  with 
the  map  from  initial  conditions. 

2.2.2  Library  of  motion  primitives 

Trim  trajectories  and  maneuvers  define  our  “library”  of  primitives  for  motion 
planning;  see  also  [6,  Section  III], 

Definition  2.2  (trim)  A  C 1  function  x  :  [0,  T]  — >■  Rn  is  a  trim  trajectory 
forV  if  there  exists  (eg,  called  the  trim  velocity  vector,  and  ji  G  Rm,  called 
the  trim  input,  such  that 

x(t)  =  \I/(exp(£f), z:(0))  for  all  t  E  [0,  T]  ,  (2) 

x(t)  =  f(x(t),ii)  for  almost  all  t  G  [0,T],  ■ 

When  the  right-hand  side  of  V  is  locally  Lipschitz,  every  trim  trajectory  x 
for  V  is  uniquely  defined  by  its  velocity  £  and  initial  condition  rr0.  We  shall 
assume  the  following  property  throughout  the  paper. 

Standing  Assumption  2.3  The  function  /  :  Rn  x  Rm  — >  Rn  is  locally 
Lipschitz  continuous.  The  nonlinear  system  V  is  invariant  under  the  action 

of®.  ■ 

Then,  for  the  nonlinear  system  V  with  symmetry  group  C?,  we  store  £  and  a;0 
in  the  set  of  trim  trajectories,  which  is  denoted  by  T(V,&)  C  x  RT 

1This  property  does  not  depend  on  the  notion  of  solution  used.  It  is  required  to  hold 
for  each  (perhaps  nonunique)  solution  to  V  on  its  domain. 
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Definition  2.4  (maneuver)  A  C1  function  x  :  [0,  T]  — >•  Rn  is  a  maneuver 
for'P  if  there  exist  a  function  6  G  VC°( R>o,Mm),  called  the  maneuver  input, 
such  that 

x(t)  =  f  (x(t) ,  j3(t))  for  almost  all  t  G  [0,Tj  ; 

g  G  Q ,  called  the  maneuver  displacement,  satisfying 


x{T)  =  V(gM 0))  ; 

and  trim  trajectories  x'  :  [0,TV|  --->  Rn,aR  :  [0,  T"]  --->  Rn  that  are  compatible 
with  x,  i.e.,  there  exist  matching  displacements  g’ ,  g"  G  Q  such  that 

x'(T')  =  V(g',x(Q)),  x(T)  =  $(/,x"(0))  .  ■ 


Remark  2.5  The  matching  displacements  g'  and  gn  in  Definition  2. 4  guar¬ 
antee  that  trim  trajectories  and  maneuvers  can  be  concatenated.  More  pre¬ 
cisely,  the  left  action  \1/  with  displacement  g'  guarantees  that  the  end  point 
of  the  ( left  compatible )  trim  trajectory  x'  can  be  concatenated  with  the  initial 
point  of  the  maneuver  x,  while  the  left  action  T  with  displacement  g"  guar¬ 
antees  that  the  initial  point  of  the  (right  compatible )  trim  trajectory  x"  can 
be  concatenated  with  the  final  point  of  the  maneuver  x.  ■ 

Maneuver  information  for  V  with  symmetry  group  Q  is  stored  in  the  set 
A By  the  regularity  properties  of  /,  a  maneuver  x  for  V  can  be  gen¬ 
erated  by  only  knowing  the  input  (5  applied  to  V  and  the  initial  condition  x°. 
By  construction,  the  application  of  fi  at  x°  causes  a  maneuver  displacement 
given  by  g  G  Q. 

Following  the  definitions  above,  a  “library”  of  motion  primitives  for  V 
with  symmetry  group  Q  is  given  by  (T(V,Q),M(V,Q)).  Let  Qt,Qm  C  N 
be  compact  and  disjoint  sets,  and  define  Q  : Qt  U  Qm-  The  set  Qt  (re¬ 
spectively,  Qm)  is  such  that  each  of  its  elements  is  uniquely  associated  to 
a  trim  trajectory  (respectively,  to  a  maneuver).  More  precisely,  for  each 
q  G  Qt ,  (£9, £„)  €  T(V,Q)  defines  the  trim  trajectory  xq(t)  ------  \L(exp (fiqt),xT) 

with  xq(0)  ------  x°q,  while  for  each  q  G  Qm ,  (i3q,xq,  gq,Tq)  G  M(V,Q)  C 

'PCu(R>o,  MiTi)  x  W1'  xgxl  correspond  to  the  input  to  generate  the  ma¬ 
neuver  xq  from  which,  after  Tq  units  of  time,  results  in  a  displacement 
given  by  gq. 


2.2.3  Motion  plan 
A  motion  plan  v  is  denoted  by 

v  :  =  { (q\  ,Tqi),  (ft ,  g'2  •  dT) ,  (ft  ,Tqs),..,, 

>---,(<&-i,9k-i,9k-i)i(Qk,Tqk)}  , 

where  k  E  N>3  is  an  odd  number  and: 

•  For  each  odd  number  j  E  N </.,  q,j  E  Qt- 

•  For  each  even  number  j  E  N<?c,  q.j  E  Qm  and  the  j-th  maneuver  is  com¬ 

patible  with  the  ( j  -----  l)-th  trim  trajectory  with  matching  displacement 
g'j  and  with  the  (j  -\-  l)-th  trim  trajectory  with  matching  displacement 
9j- 

•  For  each  odd  number  j  E  N<fc,  Ta,  E  R>0  defines  the  time  to  execute 
the  q.j- th  trim  trajectory.  The  nonnegative  constant  T0>  for  the  last 
trim  trajectory  can  be  either  finite  or  infinite. 

In  other  words,  a  motion  plan  v  is  given  by  a  sequence  where 

'02,  t q, ...  ,'Cfe-i  are  such  that  ft,  ft, ,  ft-i  €  Qm  define  maneuvers  and 

. . .  .Vk  are  such  that  ql ,  q:i . . . .  E  Qt  define  (compatible)  trim  trajec¬ 
tories.  (Alternatively,  and  without  affecting  the  results  in  this  paper,  motion 
plans  can  be  defined  as  in  |6|.)  We  denote  by  the  set  of  motion  plans 

for  V  with  symmetry  group  Q  generated  from  (T {V.  Q),  .M('P,  Q)).  Figure  1 
depicts  a  sample  trim-maneuver-trim  piece  of  a  motion  plan  v  G  V{V,Q). 


2.3  Hybrid  systems 

The  hybrid  control  framework  proposed  in  this  paper  for  maneuver- based 
motion  planning  follows  the  general  model  for  hybrid  systems  in  outlined 
in  |8j  (see  also  !9,17j).  Hybrid  systems  are  dynamical  systems  with  continuous 
and  discrete  dynamics.  In  |8],  a  hybrid  system  'H  is  given  by  a  flow  map,  a 
flow  set,  a  jump  map,  and  a  jump  set.  For  the  purposes  of  this  paper,  the 
state  of  the  hybrid  system,  denoted  by  (j,  takes  values  in  R're,  the  flow  map 
is  given  by  a  function  /  :  Rn  — >■  W1'  and  the  flow  set,  denoted  by  C  C  Rn, 
define  the  flow  equation  x  ------  f(x),x  E  C\  while  the  jump  map  is  given  by  a 

function  g  :  Rre  — >■  Rn  and  the  jump  set,  denoted  by  D  C  Rre,  define  the  jump 
equation  a:1"  =  g(x),x  E  D.  Continuous  evolution  of  the  solutions  (or  flows) 


Figure  1:  Sequence  of  entries  of  a  motion  plan  v:  Vj_\  =  defining 

trim  trajectory  xqj_ , ,  Vj  =  ( qj,gq.,gq '.)  defining  maneuver  xq.,  and  Vj+ 1  = 
(qj+1,Tq.+1)  defining  trim  trajectory  xq,.+  l. 

to  Tt  is  permitted  only  when  the  solution  is  in  C  and  discrete  evolution  (or 
jumps)  is  allowed  only  when  the  solution  is  in  D.  Hence,  a  hybrid  system  7 i 
has  data  (f,C,g,D)  and  can  be  written  as 

n :  xGRn  (  X+  =  fiX}:  XGn 

^  x+  =  g(x),  x  e  D  . 

To  define  solutions  to  the  number  of  jumps  is  treated  as  an  independent 
variable  j  and  the  state  is  parametrized  by  (£,  j).  A  solution  is  a  function 
defined  on  subsets  of  R>0  x  N.  A  subset  E  C  R>o  x  N  is  a  compact  hybrid 
time  domain  if 

j- 1 

E=  U 

j= o 

for  some  finite  sequence  of  times  0  =  to  <  ti  ...<  tj.  It  is  a  hybrid  time 
domain  if  for  all  (T,  J)  £  E,  E  fl  ([0,  T]  x  {0, 1, . . .  J})  is  a  compact  hybrid 
domain.  On  each  hybrid  time  domain  there  is  a  natural  ordering  of  points: 
(t,j)  ^  if)  if  t  <  t'  and  j  <  f.  A  hybrid  arc  is  a  function  x  :  domrr  — >  Rn 
on  a  hybrid  time  domain  doma;  such  that  x(t,j)  is  absolutely  continuous  in 
t  for  a  fixed  j  and  (t,j)  G  domx.  It  is  a  solution  to  the  hybrid  system  Tt  if 
a:(0,  0)  G  C  U  D  and 

(51)  For  all  jeN  and  almost  all  t  such  that  (t,j)  G  domr, 

x(t,  j )  G  C,  x(t,  j )  =  f(x(t,  j )) 

(52)  For  all  (t,j)  G  domx  such  that  (t,j  +  1)  G  domr, 

x(t,j)  G  D,  x(t,j  +  1)  =  g(x(t,j)). 
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A  concept  of  closeness  of  solutions  to  hybrid  systems  is  as  follows.  Two 
solutions  x  :  domrc  — >  Rn,  y  :  dom.y  — >  Rn  are  (T,  J,s)- close  if: 

(a)  for  all  (t.j)  G  domr  with  t  <  T,  j  <  J  there  exists  s  such  that 
(s,j)  G  domy,  \t  —  s\  <  e,  and 

. y(s,j) |  <  £, 

(b)  for  all  (t,j)  G  dom?/  with  t  <  T,  j  <  J  there  exists  s  such  that 
(s,j)  G  dom.r.  |t  —  sj  <  e,  and 

j y(t,f)  —  x(s,  j)|  <  e. 

Note  that  this  closeness  concept  does  not  require  solutions  to  be  close  at 
jumps  at  the  same  hybrid  instant  See  |8|  and  |9|  for  more  details. 

3  A  hybrid  controller  for  motion  planning 

Given  a  motion  plan  v  G  VCP,  Q) ,  our  goal  is  to  design  a  controller  generating 
a  trajectory  of  V  that  satisfies  the  motion  plan  specifications  given  in  terms  of 
a  finite  sequence  of  trim  trajectories  and  maneuvers  from  (T  (V,  Q)). 

We  propose  a  hybrid  controller,  denoted  by  'H.c,  with: 

•  logic  state  q  G  Q  to  indicate  the  system  mode:  trim  mode  when  q  G  Qy, 
maneuver  mode  when  q  G  Qm- 

•  logic  state  p  G  N  to  select  an  entry  of  a  given  motion  plan  v  G  V{P,  Q). 

•  displacement  state  z  G  Q  to  store  the  overall  displacement  of  the  tra¬ 
jectory  of  V. 

•  timer  state  r  G  R  to  keep  track  of  the  time  in  maneuver  mode  and  to 
parametrize  the  reference  trajectory  during  trim  mode. 


The  output  of  the  controller,  that  is,  the  input  of  P ,  is 


u==Kc(x,q,r)  (3) 

where  kc:M"xQx  R  — »  Rm.  The  input  to  lic  is  the  state  x  of  V. 


3.1  Control  strategy 


Given  a  motion  plan  v  E  V\P,CT),  let  q  :  Qj,  j  t  N<fe.  The  controller 
'Hc  performs  the  following  tasks: 

Task  1)  Trim.  Trajectory  Tracking:  Track  the  trim  trajectory  xq,  where 
xq  is  defined  by  G  TifP.Q)  via  (2). 

Task  2)  Maneuver  Execution  Start:  When  the  state  x  is  such  that  the 
maneuver  xq.+l,  which  succeeds  the  trim  trajectory  xq,  can  be  executed  and 
the  timer  elapsed  for  at  least  Tq  units  of  time,  update  q  to  qj+i,  reset  timer 
r  to  zero,  and  execute  the  ( j  +  l)-th  maneuver. 

Task  3)  Maneuver  Execution  End :  When  the  state  x  is  such  that  the 
trim  trajectory  -%u.2  can  be  executed  and  the  timer  r  has  elapsed  for  at  least 
Tq  units  of  time,  update  q  to  qj+ 2  and  perform  Task  1)  if  j  +  2  •<  k. 


Execution  of  trim  trajectories  in  Task  1  is  performed  in  closed-loop  with 
a  local  tracking  controller  that  guarantees  x(t)  --->  xa(t)  asymptotically.  Ma¬ 
neuvers  are  started  when:  1)  the  timer  has  elapsed  for  at  least  the  duration 


planned  for  the  predecessor  trim  trajectory,  and  2)  the  state  reaches  a  set 
from  where  the  maneuver  can  be  executed  (the  latter  corresponds  to  Task  2). 


The  trim  trajectory  that  follows  every  maneuver  is  started  as  soon  as  the 
state  x  is  in  the  set  where  tracking  is  possible  and  the  timer  has  elapsed  the 


specified  amount  of  time  for  the  maneuver. 


3.2  Control  design 

The  following  assumption  guarantees  that  Task  1  can  be  accomplished. 


Assumption  3.1  (tracking  of  trim  trajectories)  For  each  q  £  Qt,  there  ex¬ 
ists  a  continuous  function  Kq  :  Rw  x  R>o  — >  Rm,  a  continuously  differentiable 
function  Vq  :  Rn  R>o>  class-K. ./x>  functions  cTq1aq,  and  an  open  neighbor¬ 
hood  of  the  origin  Bg  C  Rn  such  that 


°4(!el)  ^  v?(e)  <  o;5(m) 

TOe)>/(e))  ^ 


Ve  e 
Ve  ( 


Rn 
;  B„ 


(4) 


where  f  :  Rn  Rn  is  given  by 

/(e)  =  f(e  +  xq(t),Kq(e  +  xq(t),t)  -  f(xq(t),ptq)  , 

and  defines  the  time-invariant  system,  e  -----  /(e)  invariant  under  the  action  of 
T,  where  xq  is  the  trim  trajectory  generated  by  pJq. 


Remark  3.2  In  addition  to  the  invariance  property ,  Assumption  3.1  guar¬ 
antees  the  existence  of  a  local  controller,  with  basin  of  attraction  Bq,  which  ac¬ 
complishes  asymptotic  tracking  of  trim  trajectories.  Additionally,  each  track¬ 
ing  control  law  nq  is  such  that,  when  applied  to  V ,  result  in  a  time-invariant 
error  system  with  e  :=  x  —  xq  having  the  symmetry  property.  This  assumption 
holds  for  nonlinear  systems  that  can  be  put  in  feedback  linearizable  normal 
form  [3, 10]  with  error  system  that  is  invariant  under  the  action  of  A!  [15]. 


The  construction  of  the  flow  and  jumps  sets  of  Tic  follows.  By  the  conti¬ 
nuity  properties  of  maneuvers  in  Definition  2.4,  for  each  maneuver  xq  with  in¬ 
put  Pq  and  maneuver  duration  Tq,  q  E  Qm,  there  exist  disjoint  and  open  sets 
Sg,Lq  C  Rn  such  that  for  each  :r9(0)  E  Sg,  xq(Tq)  E  Lq,  xq(t)  =  f(xg,/3g(t)). 
For  each  q  E  Qm,  pick  compact  sets  Dq  such  that  Dq  C  Sq  and  xg  E  D°,  and 
define  Cq  :=  Rn  \  Dq.  The  set  Dq,  q  E  Qm-,  corresponds  to  the  maneuver’s 
start  set  in  Task  2. 

We  now  compute  the  set  of  points  from  where  tracking  of  trim  trajectories 
is  possible.  By  construction,  there  exist  e*  >  0  such  that 

e*  :=  argmax  {x°  +  e®  C  Sq,  \fq  E  Qm}  ■ 

£>0 

Using  Assumption  3.1,  for  each  q  E  Qt ,  define 

Dq  :={e  E  Rn  |  Vq(e)  <  cq}  , 
where  cq  >  0  is  such  that 

Dq  C  (x°q  +  (5,1)  n  Bq  ,  Sq  :=  Kr^expCT,)^*))  , 

and  (q;^)-1  is  the  inverse  of  the  function  a ].  Define  Cq  :=  \  Dq.  This 

construction  yields  a  constant  5g  such  that  when  the  trim  trajectory  xg(t)  is 
tracked  from  initial  conditions  in  Dq ,  the  state  x  belongs  to  a  subset  of  the 
start  set  of  each  of  the  maneuvers  in  Ai(V ,<?)  after  Tg  units  of  time  have 
elapsed  (Tq  is  the  execution  time  of  the  trim  trajectory  given  in  the  motion 
plan) . 

The  following  assumption  guarantees  that  maneuvers  take  trajectories  to 
points  where  trim  trajectories  can  be  executed. 
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Assumption  3.3  (nested  condition)  For  every  motion  plan  v  G  V(V,  Q), 
every  maneuver  with  associated  entry  Vi  in  v  and  input  /3q.,  its  associated  set 
Lq.  is  such  that 

Lq-i  C  Dg.+  1  , 

where  Dqi+1  is  the  set  associated  with  tracking  of  the  trim  trajectory  xq.+1, 
Qi+ 1  £  Qt- 


Remark  3.4  The  condition  in  Assumption  3.3  assures  that ,  after  a  maneu¬ 
ver,  the  state  x  is  in  a  set  from  which  tracking  of  the  trim  trajectory  succeeding 
it  is  possible.  This  condition  holds  by  picking  small  enough  landing  set  Lq 
when  Assumption  3.1  is  in  place.  However,  in  order  to  get  practical  robust¬ 
ness  results,  the  landing  sets  are  usually  fixed.  In  such  cases,  the  tracking 
law  in  Assumption  3.1  should  be  chosen  to  have  large  enough  set  Dq,  q  G  Qt- 


Figure  2  illustrates  the  sets  designed  above. 


Figure  2:  Sets  of  the  hybrid  controller  for  a  trim  trajectory  and  maneuver  in 
the  motion  primitive  in  Figure  1. 


3.3  Hybrid  controller 

The  control  logic  outlined  above  is  implemented  in  the  hybrid  controller  fic 
as  follows. 
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3.3.1  Jumps 


Jumps  occur  while  in  trim  mode  with  p  <  k  (i.e.,  it  is  not  the  last  trim 
trajectory  of  the  motion  plan)  when  the  state  x  reaches  the  set  of  points  where 
the  maneuver  xqp,_,  can  be  started  and  the  timer  r  has  elapsed  for  Tqv  units  of 
time.  The  set  in  the  first  condition  is  given  by  Dqv+1 ,  gv+i  £  Qm,  after  the  left 
action  T  with  displacement  given  by  z  multiplied  by  the  nominally  expected 
trim  trajectory  displacement  exp(£9T9)  and  the  matching  displacement  g'  r 
Then,  jumps  occur  when 


g  €  Qt  and  x  £  '&(z  exp(£qTq)grqp+i ,  Dqp+i)  and  r  >  Tq 
with  update  law 


(5) 


q  ;  =  (/,, .  i .  p+  =  p+1,  =  zexp(ZqT),  r+  =  0  ,  (6) 

that  is,  q  is  mapped  to  the  next  mode  in  the  motion  plan  v,  the  motion 
plan  index  p  is  incremented  by  one,  z  is  updated  with  the  current  total 
displacement  of  the  motion  primitive,  and  r  is  reset  to  zero. 

While  in  maneuver  mode,  jumps  occur  when  the  state  reaches  the  set  of 
points  where  the  trim  trajectory  xqt+,  can  be  started  and  the  timer  state  r 
has  elapsed  for  at  least  Tq  units  of  time.  As  in  the  case  for  jumps  during 
trim  mode,  the  set  in  the  former  condition  is  given  by  Dq,  q  £  Qm,  after 
the  invariant  operation  T  with  displacement  given  by  z  multiplied  by  the 
planned  maneuver  trajectory  displacement,  which  is  given  by  gq,  and  the 
matching  displacement  gq.  Then,  jumps  in  maneuver  mode  occur  when 


Q  £  Qm  X  t:  $  [  ZQqQ^  7  l)q 

B+l)  and  T  Tq  : 

(7) 

with  update  law 

Q+  =  QP+u  P+=P+l,  z+ 

=  zgq,  r+  =  0  . 

(8) 

3.3.2  .Flows 

During  flows,  the  controller  variables  have  c 

g  =  (j,  p=  0,  z  =  0 

lynamics  given  by 

,  f  =  1  , 

(9) 

when 

q  £  Qt  and  (x  £  $  (z  exp(£qTq) g!qp+ 

„Cw,)orT6[0,r,]), 

(10) 

or 

q  £  Qm  and  (x  £  \ V(zgqgq,Cqf 

,+.)  or  re  [0,r,]). 

(11) 
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3.3.3  Output 

The  controller  output  is  the  input  to  V  and  is  given  by  u  =  Ke(x,q,r )  where 


kc{x,  q,  t) 


fiq(T) 

Kq(x,T) 


if  g  G  Qm 
if  q  G  Qt  . 


(1 


The  function  6q  is  the  control  input  that  generates  the  g-th  maneuver,  q  G 
Qm-  The  function  Kq  is  the  tracking  control  law  in  Assumption  3.1  for 
the  g-th  trim  trajectory,  q  G  Qt,  which  is  designed  using  trim  trajectory 
information. 


3.3.4  Closed-loop  system 

We  denote  the  closed-loop  system  resulting  from  controlling  P  with  7ic  by 
Tid  and  its  state  by  ip  :=  ( x,q,p,z,r )  G  X  :=  En  x  Q  x  M<fc  x  R1  x  R, 
where  the  Euclidean  space  R*  embeds  Q.  The  continuous  dynamics  are  given 
by  closed- loop  plant  dynamics  x  =  fix,  kc(x,  q,  r))  along  with  (9),  with  flow 
set  given  by  the  union  of  the  sets  defined  by  (10)  and  (11).  The  discrete 
dynamics  are  given  by  the  update  laws  in  (6)  and  (8).  The  resulting  closed- 
loop  system  ?4j  can  be  written  in  the  compact  form  in  (2.3)  using  ip  as  the 
state  and  appropriately  defining  functions  f,g  and  sets  C  and  I). 


4  Motion  plan  execution: 

nominal  and  perturbed  case 

Given  a  motion  plan  v  and  an  initial  configuration  (.x)),yT  G  Rra  x  Q  such 
that  =  T(g)?,xDJ,  let;  r  ;  clomr  — >  Rn  describe  the  desired  trajectory  of 


the  nominal  motion  plan  v,  that  is: 


Al(grfixqi(t)) 


if  t  E  [0,  Til , 
and  j  =  0 


,  n  if  £  G  \T\,  Tfl , 

$  (gv  exp  y/,  7j )  <?-, ,  xg2  yt)  j  and  =  {  ' ' 


U/fiexphfTT 


,X 


?3 


if  t  e  [T2,r3], 
and  j  =  2 


if 

•  •  •  exp(4j».._  j  Tk- ! )  g'l ,  xqk(t)) 


W(^exp(^:l2V)^... 


where  xqi  is  the  trim  trajectory  with  (qqi ,  :r|/)  G  T yP,  Q).  xg2  is  the  maneuver 
with  {Pq2,xq2,gq,,/Tg,,)  G  .h4yP,Q),  etc.  Note  that  each  jump  of  r  corresponds 
to  a  change  of  motion  primitive.  For  example,  for  each  (£,  j)  G  |0,Tj]  x  {0}, 
r{t,j)  is  given  by  the  gi-th  trim  trajectory,  and  after  the  jump  at  t  =  7  j ._/  = 
0,  and  for  all  (£,/)  G  [Tj.T^I  x  {!},  is  given  by  the  cfe-th  maneuver. 

The  duration  of  the  motion  plan  v  is  Tr  =  J2*=i  3  +  J2i= 24  h-i^qr 

When  Tr  is  finite,  domr  is  a  subset  of  [0,Tr]  x  {0, 1,  2, . . . ,  k  —  1},  while  when 
Tr  is  infinite,  domr  is  a  subset  of  1 0, 00 )  x  {0, 1, 2, . . . ,  k  —  1}. 

Theorem  4.1  ( nominal  execution)  Let  Assumptions  3,1  and  3,3  hold.  For 
each  v  E  V(V,Q)  with  nominal  motion  plan  trajectory  r  and  each  (x')ng'),)  E 
Rn  x  Q  such  that  x®  =  (€qx,x%)  £  T(P,Q),  there  exists  a  unique 

solution  <p  to  Ltd  from  <p(0,0)  =  (x))nqi,  1,  <?/,  0)  that  is  hounded  and  is  such 
that  the  x  component  satisfies  x(t,j)  =  r{t,j)  for  all  (£,  j)  E  domsp. 


Remark  4.2  Theorem  4-1,  which  follows  by  construction ,  states  that  every 
motion  plan  v  E  V(fP,  Q)  is  properly  executed  by  Ltd-  This  result  recovers  the 
nominal  motion  plan  execution  property  of  the  hybrid  automaton  in  j6j.  ■ 

In  addition  to  the  nominal  property  in  Theorem  4.1,  the  proposed  hybrid 
control  construction  guarantees  that,  under  the  presence  of  perturbations, 
motion  plan  execution  stay  close  to  a  nominal  one.  Note  that  the  presence  of 
perturbations  in  Llci  on  the  initial  conditions,  parameters,  and/or  the  state 
affects  the  jump  times.  In  this  way,  the  domain  of  the  resulting  trajectory 


does  not  need  to  coincide  with  the  domain  of  the  nominal  trajectory  r  as¬ 
sociated  to  v  E  V(V,Q).  The  (T,  J, e) -close ness  notion  of  distance  between 
hybrid  arcs  in  Section  2.3  handles  such  a  situation. 

Theorem  4.3  (perturbation  of  initial  conditions)  Let  Assumptions  3.1  and 
3.3  hold.  For  each  v  E  VfiP,G)  with  nominal  motion  plan  trajectory  r  and 
each  (x e  x  Q  such  that  x°v  =  $(p£,:r°  ),  (tqi,x0qi)  E  T(V,G),  each 
£  >  0,  each  compact  set  K  C  BqiJ  and  each  (T,  J)  £  R>o  x  N,  (T,J)  < 
(Tr,  k  —  1),  there  exists  5  >  0  such  that  every  solution  <ps  to  Lid  with  <ps(0:  0)  = 
(^, pi,  1, <7°, 0),  x°5  E  K  +  is  bounded  and  the  x  component  and  r  are 
(T,  J,  e) -close. 

Remark  4.4  The  time  horizon  (T,  J)  where  the  closeness  property  in  Theo¬ 
rem  4.3  holds  can  be  picked  to  be  equal  to  (Tr,  k  —  1)  when  Tr  is  finite.  Then, 
closeness  between  the  component  x  of  the  solution  and  r  is  guaranteed  in  the 
entire  duration  of  the  motion  plan.  The  hybrid  time  domain  of  each  solution 
to  TCd  can  be  extended  to  an  unbounded  one  without  affecting  the  behavior  of 
the  system  up  to  time  (T,  J) .  In  addition  to  the  regularity  properties  of  the 
closed-loop  system  (guaranteed  by  the  standing  assumption  and  the  hybrid 
controller  construction) ,  the  proof  of  Theorem  4-3  extends  the  hybrid  time 
domain  to  an  unbounded  one  to  enable  the  application  of  results  in  [9]  for 
hybrid  systems  with  perturbations.  ■ 

Under  the  presence  of  perturbations,  system  V  controlled  by  TL  can  be 
written  as 

x  =  f(x,Kc(x  +  d1(t),q,T))+d2(t)  ,  (13) 

where  d\  corresponds  to  error  in  the  measurements  of  x  and  o?2  models  other 
exogenous  disturbances  and  unmodeled  dynamics.  The  addition  of  these 
perturbations  in  the  closed-loop  system  Hci  results  in  a  perturbed  hybrid 
system,  denote  as  Ttci ,  which  can  be  written  as 

<fi  =  f  (jp  +  di(t))  T  (£)  ip  -\-  d±  E  C 
T+  =  Kv)  <p  +  di  E  D  , 

The  following  result  asserts  that  the  motion  planning  is  robust  to  a  class 
of  perturbations.  2 

2 The  exogenous  signals  d\  and  d 2  are  given  on  hybrid  time  domains  (given  a  hybrid 
time  domain  S  and  an  exogenous  signal  d\(t ),  we  can  define,  with  some  abuse  of  notation, 
d\ (t,j)  :=  di(t)  for  each  (t,j)  £  S.)  Solutions  to  hybrid  systems  with  the  perturbations 
above  is  understood  similarly  to  the  notion  of  solution  outlined  in  Section  2.3. 
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Theorem  4.5  (perturbations)  Let  Assumptions  3.1  and  3.3  hold.  For  each 
v  G  V(V,  <3)  with  nominal  motion  plan  trajectory  r  and  each  (x°,  g °)  G  Rn  x  Q 
such  that  x®  =  xj^),  G  T(V,G),  each  e  >  0,  each  compact  set 

K  C  Bqi,  and  each  ( T ,  J )  G  R>o  x  N,  (T,  J )  ■<  ( Tr,k  —  1),  there  exists  5  >  0 
such  that  every  solution  <p  to  Ltd  with  <p(0, 0)  =  (x°,gi,  1,5°,  0),  x°  G  K+5B>, 
\di(t,j)\  <  6  and  \d2(t,j)\  <  5  for  each  ( t,j )  G  dom< p,  is  bounded  and  the  x 
component  and  the  motion  plan  trajectory  r  are  (T,  J,s) -close. 


Remark  4.6  The  proof  of  this  result  uses  a  technique  from  [9,  Section  V] 
in  which  a  perturbed  hybrid  system  Ltd  is  embedded  into  a  set-valued  hybrid 
system.  Using  the  hybrid  time  domain  extension  as  in  Theorem  4.3,  the 
results  follows  from  [9,  Corollary  5.5].  ■ 


Figure  3:  Motion  primitive  (dashed)  in  Figure  1  and  simple  airplane  trajec¬ 
tory  resulting  from  applying  our  hybrid  control  strategy  for  motion  planning. 
Tracking  control  during  trims  (red  pieces)  guarantees  that  solution  and  trim 
trajectory  are  stay  close.  Maneuver  starts  from  a  point  nearby  the  maneuver 
(blue  piece)  in  the  library  and  remains  close  to  it. 

Finally,  Figure  3  illustrates  a  solution  to  Ttd  starting  nearby  the  motion 
plan  in  Figure  1.  This  corresponds  to  a  simulation  result  from  a  toolbox  for 
robust  maneuver-based  motion  planning,  currently  under  development. 
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5  Conclusion 


We  presented  a  hybrid  systems  framework  for  maneuver-based  motion  plan¬ 
ning;  algorithms  for  nonlinear  systems  with  symmetries.  We  systematically 
described  the  construction  of  a  hybrid  controller  and  showed  its  robustness 
properties  for  a  large  class  of  perturbations.  Our  results  are  built  upon  recent 
tools  for  robustness  of  stability  for  hybrid  systems.  'Extensions  of  the  hybrid 
control  strategy  to  situations  where  bounds  on  the  perturbations  are  known 
beforehand  follow  from  the  ideas  presented  in  this  manuscript  and  will  be 
closely  explored  in  the  future. 
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1  Overview 

The  paper  by  Velenis,  Tsiotras,  and  Lu  [6]  is  a  very  interesting  contribution  towards  the  development  of  au¬ 
tomatic  control  systems  able  to  push  the  performance  envelopes  of  autonomous  vehicles  and  mobile  robots. 
In  fact,  while  automatic  control  techniques  routinely  show  superior  performance  (with  respect  to  humans), 
e.g.,  in  terms  of  set-point  regulation,  the  dexterity  and  nimbleness  demonstrated  by  human-operated  vehicles 
and  machines  is  as  yet  largely  unattainable  by  automated  systems. 

Advances  towards  the  development  of  autonomous  aircraft  capable  of  performing  human-inspired  acro¬ 
batic  maneuvers  have  been  reported,  e.g.,  in  [4,5].  The  work  of  Velenis  et  al  is  a  first  step  in  the  context 
of  Ackermann-steered  (i.e.,  car-like)  ground  vehicles,  with  a  special  emphasis  on  techniques  applicable  to 
driving  on  loose  terrain. 

Both  [4]  and  [6]  follow  a  similar  basic  process:  (i)  data  are  collected  from  an  instrumented  vehicle,  while 
an  expert  human  pilot  executes  a  maneuver  of  interest;  (ii)  these  data  are  used  to  validate  an  analytical  model 
of  the  vehicle’s  dynamics,  applicable  to  the  operating  conditions  encountered  during  the  maneuver;  (iii)  the 
data  are  further  interpreted  to  parameterize  and  design  a  control  strategy  amenable  to  implementation  on  a 
model-based  control  strategy;  (iv)  such  control  strategy  is  finally  assessed  on  a  high-fidelity  simulation. 

A  significant  difference  (aside  from  the  application  domain)  is  that  Velenis  et  al  investigate  the  optimal¬ 
ity  of  the  maneuver,  and  develop  a  parameterization  of  control  strategies  to  reduce  the  search  space  of  a 
nonlinear  programming  algorithm.  This  enables  them  to  extend  the  applicability  of  the  maneuver  beyond 
the  original  scenario:  for  example,  they  are  able  to  compute  trail-braking  maneuvers  through  a  variety  of 
corners,  with  different  total  turning  angles. 

2  Towards  a  symbolic  approach  to  autonomous  high-speed  driving 

As  argued  in  [6],  the  development  of  methods  for  the  design  of  control  laws  to  perform  a  certain  class  of 
maneuver  is  motivated  by  the  prospect  of  building  a  library  of  such  maneuvers,  and  then  stringing  together 
such  maneuvers  in  such  a  way  to  construct  more  complicated  trajectories.  This  is  the  basis  concept  behind  a 
promising  new  research  direction  in  the  literature  on  robotics  and  automatic  control,  which  is  often  referred 
to  as  symbolic  control: ;  see,  e.g.,  [2]  for  a  general  introduction. 

A  formal  approach  to  the  intuitive  concept  described  above  was  presented  in  [3].  The  key  property  of  a 
dynamical  system  enabling  such  an  approach  is  symmetry ,  i.e.,  invariance  to  a  certain  class  of  transforma¬ 
tions  on  the  state  of  the  system.  This  is  a  very  general  property  of  man-made  vehicles:  for  example,  the 
dynamics  of  a  car-like  vehicle,  operating  on  flat,  horizontal  terrain,  are  invariant  with  respect  to  rigid-body 
motions  on  the  horizontal  plane.  The  existence  of  symmetries  in  a  dynamical  system  allows  the  definition  of 
so-called  “motion  primitives,”  i.e.,  equivalence  classes  of  trajectories  modulo  the  symmetry  transformations. 

In  [3],  in  order  to  develop  a  systematic  approach  to  the  selection  of  motion  primitives  to  include  in  a 
library,  two  kinds  of  motion  primitives  were  identified:  trim  trajectories  and  maneuvers.  The  former,  also 
called  relative  equilibria  or  steady-state  trajectories,  correspond  to  orbits  of  the  infinitesimal  action  of  the 
symmetry  transformation:  for  example,  in  the  case  of  a  car-like  vehicle,  such  trim  trajectories  correspond 
to  circles  described  at  constant  speed,  steering  angle,  and  throttle  settings  (this  includes  degenerate  circles, 
such  as  straight  lines).  Maneuvers  are  then  defined  as  transitions  between  such  trim  trajectories.  The  ap¬ 
proach  in  [6]  can  in  fact  be  seen  as  a  way  to  compute  optimal  maneuvers:  in  particular,  the  trail-braking 
maneuver  described  in  the  paper  can  be  thought  of  as  a  transition  between  a  trim  trajectory  in  which  the  car 
is  moving  straight  at  constant  (high)  speed,  back  to  the  same  trim  trajectory,  with  a  different  initial  point, 
and  a  different  heading. 


The  objective  of  the  remainder  of  this  note  is  complementary  to  [6]:  instead  of  computing  maneuvers,  we 
will  analyze  trim  trajectories  for  the  same  dynamic  model.  The  purpose  of  this  investigation  is  not  only  to 
provide  well-defined  “starting”  and  “ending”  states  for  trail-braking  (or  other)  maneuvers,  but  also  to  inves¬ 
tigate  possible  ways  to  further  decompose  such  maneuvers.  For  example,  one  could  imagine  decomposing  a 
trail-braking  maneuver  into  (i)  an  entry  phase,  in  which  the  car  brakes  hard  and  then  steers  to  enter  a  circular 
trajectory  at  a  high  sideslip  angle;  (ii)  a  steady-state  phase,  in  which  the  car  makes  progress  around  the 
corner,  on  a  tight  circle;  and  (iii)  an  exit  phase,  in  which  the  car  countersteers  while  accelerating  out  of  the 
turn.  Varying  the  length  of  the  “trim”  phase,  the  car  would  be  able  to  execute  a  whole  class  of  trail-braking 
turns,  through  a  range  of  angles — without  the  need  to  compute  explicitly  several  different  trajectories. 

3  Trim  trajectories  for  the  half-car  model 

Recall  that  since  the  dynamics  of  the  car  are  invariant  to  rigid  body  motions  in  the  plane,  trim  trajectories 
are  (arcs)  of  circles,  followed  at  constant  speed.  It  is  convenient  to  rewrite  the  half-car  dynamics  equations 
in  [6]  in  a  reference  frame  whose  origin  is  moving  on  a  circle  of  radius  r  at  constant  speed  V ,  rotating  at 
angular  velocity  a>  =  V  jr,  and  with  (centripetal)  acceleration  a  —  co2r .  The  x  axis  of  this  frame  is  aligned 
with  the  velocity  vector.  Using  the  same  notation  as  in  [6],  we  get 

m  ( x-  2 6)y  -  (02x)  =  fyx cos( y/ +  8)  -  fyy  +  8)  +  cos  yr -  f&y  sin  yr, 
m  (y  +  2o)x—  (02y  + 1 o2r )  =  fyx sin(yr +  8)  +  fyy  cos (yr  +  8)  +  /r*  sin  yr + fRy  cos  y (1) 
hv  =  {fpx  sin  $  H"  fpy  cos  <5)t?p  —  /r/r  . 

For  the  motion  of  the  vehicle  to  remain  planar,  the  following  balance  equations,  and  constraint  force  in¬ 
equalities,  must  be  satisfied: 

0  =  fcz  +  hz-mg , 

0  =  Kfax  cos  8  -  fyy  sin  8  ^  /Rx)  +  fpz£F  -  /rz£r  ,  (2) 

0  =  Mx  +  h(fpx  sin  8  +  fpy  cos  8  +  fay)  ? 

w 

/f*>0,  /r*>0,  \Mx\  <  mg—.  (3) 

(Note  that  the  first  two  constraints  (3)  are  usually  satisfied  for  full-size  vehicles.  The  third  one — in  which  w 
is  the  width  of  the  wheelbase,  and  Mx  is  the  reaction  moment  along  the  vehicle’s  longitudinal  axis — could 
be  violated  for  large  lateral  accelerations  occurring  in  vehicles  with  w/2h  >  1  (e.g.,  trucks  or  SUV’s)  and 
indicates  the  onset  of  roll-over  phenomena.) 

The  friction  forces  /px,  fyy,  /r*,  and  can  be  found  multiplying  the  constraint  forces  fpz  and  by  an 
appropriate  friction  coefficient,  which  in  turns  depends  on  the  amount  of  slippage  between  the  tire  and  the 
terrain.  A  popular  model  is  Pacejka’s  “Magic  Formula”  [1],  which  can  be  written  in  its  basic  form  as 

/il(s)  =  D  sin  (C  arctan  (. Bs ) ) ,  (4) 

where  B ,  C,  and  D  are  appropriate  constants,  and  s  measures  the  slip  ratio. 

For  the  purpose  of  this  note,  as  in  [6],  the  slip  ratio  is  defined  as  the  ratio  between  the  relative  speed  of  the 
wheel  and  of  the  terrain  at  the  contact  point,  and  the  speed  of  the  contact  point  on  the  wheel  in  the  absence 
of  driving/braking  torques.  With  this  definition  in  mind,  we  obtain 

s  =  y^!  +  (tana)2? 

where  a  is  the  wheel  slip  angle,  formed  by  the  longitudinal  axis  of  the  wheel  with  the  velocity  vector  at  the 
contact  point,  and  sx  is  the  longitudinal  slip,  defined  as 

<Avh eel  Awheel  1 

Sx  =  —r, - 1  • 

V  cos  a 


2 


Lateral  and  longitudinal  friction  coefficients  are  recovered  as 


Hx  (sx,s)  = 

S 

tan  a 

fiy(a,s)  =  (s), 

and  finally  we  obtain  the  lateral  and  longitudinal  friction  force  components  as  fx  =  fixfz ,  fy  =  fiyfz 
For  simplicity,  we  will  consider  the  front  and  rear  longitudinal  slip  ratios  spx  and  sr*  as  independent 
control  inputs  (in  place  of,  e.g.,  torques  on  the  wheel). 

3.1  Computing  steady-state  trajectories 

Steady-state  trajectories  correspond  to  equilibrium  points  in  the  rotating  frame,  and  can  be  found  solving 
the  system  of  equations  obtained  from  (1),  (2),  and  (3),  setting  x,  y,  y/,  and  their  derivatives  to  zero. 

Some  algebra  leads  to 

9  _  4 


f&y  =  nuo  rcos  y/ 


fRz  =  m 


and 


fFz  =  m 


4  +  4 7 

gtR  +  hoPrsmy/ 

(4+4) 

g4  —  h(02r  sin  y/ 


(5) 

(6) 


(4  +4) 

Combining  (5)  and  (6),  we  get  that  the  rear-wheel  lateral  friction  coefficient  fi&y  is 

fRy  _  F  cos  y/ 


fiRy  = 


fRz 


Since  by  definition 


Mr y  = 


g4  +  ho)2rsm  y/ 
tanaR 


sr 


-jW(^r), 


(V) 


(8) 


equations  (7)  and  (8)  can  be  solved  for  sr  as  a  function  of  the  vehicle’s  linear  and  angular  velocity  V  and  co , 
and  of  the  sideslip  angle  y/.  In  particular,  if  we  consider  the  case  of  a  front-wheel  drive  (FWD)  vehicle,  and 
assume  that  it  is  not  braking,  then  vrx  =  0  and  hence  sr  =  tanofR.  In  this  case,  (7)  and  (8)  simplify  to 


of2  Hr  cos  y/ 


g4  +  ha>2r  sin  y/ 

The  total  front- wheel  slip  ratio  can  be  computed  by  solving 


=  M  (tan  Or). 


(9) 


H(sf)  = 


fvx  +  fly  A/  ™2a)Ar2  +  fly  ~  2m(OrfRy  cos  y 


fpz 


fpz 


(10) 


for  sp.  The  steering  angle  S  necessary  to  achieve  the  above  slip  ratio  can  be  found  as  a  solution  of  the 
following  equation: 


tan(<5  —  So)  m(D2rcos(y/-\-S)  +  JnycosS 


fix  +  fly 


(ID 


where  So  is  the  steering  angle  for  which  the  wheel  slip  angle  is  zero.  Finally,  the  front-wheel  longitudinal 
slip  ratio  can  be  recovered  as 


srx  —  \  f  s p  (tan  ct f  )2  • 
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The  procedure  outlined  above  can  be  used  to  compute  the  sideslip  angle  y/,  steering  angle  <5,  and  longi¬ 
tudinal  slip  on  the  driving  wheel  spx  given  a  desired  trim  condition  (V,  a>).  This  calculation  requires  solving 
three  nonlinear  equations,  namely,  (9),  (10),  and  (11).  These  three  scalar  equations  can  be  solved  in  se¬ 
quence,  using  efficient  numerical  procedures.  However,  for  a  given  pair  (V,  a>),  there  may  be  no  solution,  or 
more  than  one  solutions. 

In  Figure  1,  the  region  in  the  (V,a)  plane  for  which  at  least  one  solution  exists,  with  \y/\  <  90°,  and 
| <5 1  <  30°,  and  |^px|  <  1  (of  these  constraints,  only  the  one  on  S  was  binding  in  some  cases).  This  figure 
reveals  an  interesting  structure  in  the  set  of  achievable  trim  conditions  for  a  FWD  vehicle. 

The  bulk  of  trim  conditions  can  be  found  in  the  region  (approximately)  bounded  on  the  left  by  the  parabola 
a  =  rk(30°)v2,  where 

is  the  kinematic  turn  radius,  computed  assuming  that  no  wheel  slipping  occurs.  In  the  case  athand,  rk(30°)  = 
4.94  m.  This  region  indicates  conditions  where  wheel  slipping  is  moderate,  and  does  not  play  a  major  role 
in  determining  the  behavior  of  the  car — except  possibly  for  the  understeer  noticeable  at  high  speeds. 
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Figure  1:  Region  of  attainable  trim  conditions  for  the  Front-Wheel  Drive  vehicle  considered  in  the  text. 
The  contour  plot  shows  the  sideslip  angle  y/  (in  degrees)  required  to  maintain  a  trim  condition  indicated  by 
linear  velocity,  and  centripetal  acceleration  (such  angle  is  not  necessarily  unique  at  all  points  in  the  region). 
The  circular  markers  indicate  the  kinematic  limit  on  the  turning  radius. 

On  the  other  hand,  to  the  left  of  the  parabola  mentioned  above,  there  exist  trim  conditions  in  which  the 
vehicle’s  center  of  mass  travels  on  a  circle  with  radius  smaller  than  the  minimum  kinematic  turning  radius. 
The  sideslip  angle  is  very  large,  of  the  order  of  25  degrees  in  our  case,  and  so  is  the  steering  angle.  This 
set  of  trim  trajectories  corresponds  to  what  are  colloquially  referred  to  as  “doughnuts,”  and  the  ability  of  the 
vehicle  to  achieve  these  trajectories  can  only  be  modeled  taking  into  account  the  effects  of  wheel  slipping 
on  the  friction  forces. 
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4  Conclusions 

In  this  note,  we  aimed  at  complementing  the  paper  under  discussion  by  computing  the  set  of  attainable  trim 
trajectories  for  the  half-car  model.  It  is  shown  that  this  set  is  far  from  trivial,  and  contains  some  trajectories 
corresponding  to  large  sideslip  angles,  and  significant  slipping/skidding  conditions.  These  trajectories  ex¬ 
ceed  kinematic  limits  on  the  minimum  radius  of  curvature,  and  on  the  maximum  angular  velocity  (and  hence 
steering  effectiveness)  achievable  by  the  car.  It  is  believed  that  such  trajectories  can  be  used,  in  concert  with 
the  techniques  developed  by  Velenis,  Tsiotras,  and  Lu,  as  further  building  blocks  to  construct  systematically 
new  classes  of  “maneuvers,”  enabling  autonomous  cars  to  design  and  execute  on-line  turns  at  the  limit  of 
their  performance. 

While  the  real-world  implementation  of  such  algorithms  on  a  full-size  car  is  still  difficult — especially 
because  of  the  difficulties  in  making  available  to  an  on-board  computer  all  the  sensory  cues  used  by  drivers, 
ranging  from  visual  and  auditory  data  to  tactile  feedback  from  the  steering  wheel — the  paper  under  discus¬ 
sion  and  this  note  provide  further  steps  towards  a  good  understanding  of  the  fundamental  geometric  and 
dynamic  properties  of  the  dynamics  of  car-like  vehicle  on  loose  terrain. 
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